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ABSTRACT 

This  study  investigates  the  utility  of  rule-based  coordination  of  motion  for  ternary-type  terrain  locomotion 
by  a  hexapod  walking  machine.  The  ternary-type  terrain  considered  is  composed  of  permitted  areas, 
forbidden  areas,  and  ditch  areas.  The  logic  for  generating  motion  coordination  is  written  in  Prolog  while  the 
simulation  of  the  terrain  and  of  the  vehicle  kinematics,  as  well  as  low-level  on-board  computer  functions, 
are  written  in  extended  Common  Lisp  and  Flavors.  It  is  found  that  this  approach,  which  utilizes  multiple 
programming  paradigms  for  programming  motion  coordination  logic  and  simulation  objects,  results  in  code 
that  is  much  easier  to  understand  and  modify  than  previous  motion  coordination  programs  written  in  Pascal. 
Thus,  the  code  development  effort  and  time  are  greatly  reduced.  The  authors  believe  that  both  the 
methodology  and  the  motion  coordination  logic  presented  in  this  report  possess  sufficient  merit  to  justify 
full-scale  physical  testing  in  the  Adaptive  Suspension  Vehicle  at  the  Ohio  State  University. 


1.   Introduction 

The  Adaptive  Suspension  Vehicle  (ASV)  is  a  large  six-legged  vehicle  designed  for  outdoor 
operation  in  rough  terrain.  Limb  motion  coordination  for  the  ASV  is  accomplished  by  an  on-board 
computer  network  consisting  of  one  PC-AT,  eight  Intel  single-board  computers,  and  two  special  purpose 
computers  [1,2].  The  software  system  is  hierarchically  organized  with  a  clear  distinction  being  made  among 
an  individual  leg  control  level,  a  leg  motion  coordination  level,  and  a  body  motion  planning  level  [2,3]. 
Except  for  the  two  special  purpose  computers,  the  application  software  for  the  ASV  is  currendy  written 
almost  entirely  in  Pascal.  A  custom  designed  real-time  operating  system,  written  mainly  in  PL/M, 
coordinates  the  functioning  of  all  processes  running  on  the  various  processors  of  the  vehicle  computer.  The 
total  ASV  software  system  involves  somewhat  more  than  150,000  lines  of  code  [2,4]. 

An  important  feature  of  the  ASV  is  its  omni-directional  motion  capability  [1,2]  which  gives  it  the 
general  maneuverability  characteristics  of  a  helicopter.  This  behavior  is  achieved  by  providing  the  operator 
with  a  joystick  with  three  major  motion  axes  for  control  of  vehicle  forward  velocity,  lateral  velocity,  and 
turning  velocity  respectively  [2].  The  vehicle  control  computer  accepts  these  commands  and  synthesizes  a 
sequence  of  leg  movements  to  produce  the  desired  body  behavior.  It  is  assisted  in  this  task  by  information 
from  an  optical  terrain  scanner  which  provides  a  map  of  terrain  elevation  in  the  immediate  vicinity  of  the 
vehicle  [5],  and  by  force  and  position  feedback  from  each  leg. 

Until  now,  nearly  all  outdoor  experiments  with  the  ASV  have  made  use  of  a  tripod  gait  in  which 
legs  are  used  in  two  sets  of  overlapping  tripods  [6,7].  This  gait  was  chosen  both  for  its  relative  simplicity 
and  for  its  known  optimality  under  high  speed  straight-line  locomotion  conditions  [7,8,9].  However,  the 
tripod  gait  is  not  well  suited  to  extreme  terrain  situations  in  which  a  significant  fraction  of  the  area  under  a 
given  leg  may  be  unsatisfactory  for  load  bearing  due  to  the  presence  of  rocks,  holes,  obstacles,  soft  soils, 
etc.  In  the  latter  case,  simulation  experiments  [10  ,11],  and  initial  indoor  testing  [12],  indicate  that  on-line 
optimization  of  leg  sequencing  should  give  better  results. 

Gaits  involving  real-time  optimization  of  stability  or  maneuverability  in  the  presence  of  terrain 
constraints  are  often  called  free  gaits  to  distinguish  them  from  the  periodic  gaits  used  by  walking  machines 
and  animals  in  less  difficult  circumstances  [13,14].  Until  now,  all  free  gait  studies  have  been  performed 


based  on  a  binary  terrain  model  which  includes  two  types  of  terrain  objects;  i.e.,  obstacle  and  non-obstacle 
[10,1 1 ,15,16]  .  The  size  of  the  individual  obstacles  is  restricted  to  be  comparable  to  that  of  the  feet  of  the 
ASV  with  the  further  assumption  that  the  obstacles  are  randomly  distributed  on  the  terrain.  In  this  study, 
in  addition  to  the  above  small  obstacles,  a  ditch  obstacle,  which  is  a  large  and  structured  obstacle  whose  size 
is  comparable  to  that  of  the  vehicle,  is  introduced  as  a  third  type.  Therefore,  the  terrain  model  is  ternary 
rather  than  binary,  and  contains  both  randomly  distributed  small  obstacles  and  large  ditch  obstacles. 
Though  the  free  gait  motion  coordinator  developed  in  [16]  performs  well  for  binary  terrain,  in  this  report,  a 
new  motion  coordinator,  called  the  "Ditch  Crossing  Motion  Coordinator"  is  additionally  introduced  to 
enhance  ditch  crossing  capability.  Thus,  two  motion  coordinators  coexist  in  the  program,  while,  at  one 
instance,  only  one  of  them  is  allowed  to  control  the  vehicle  depending  on  terrain  conditions.  If  the 
existence  of  a  ditch  is  detected,  the  newly  introduced  motion  coordinator  takes  over  control  of  the  vehicle 
until  the  ASV  has  crossed  the  ditch.  After  crossing  the  ditch,  vehicle  control  is  automatically  transferred 
back  to  the  Free  Gait  Motion  Coordinator  developed  in  [16]  to  handle  small  obstacles  effectively. 

Differing  from  all  ASV  experiments  using  an  imperative  language  (Pascal)  to  encode  stepping 
algorithms,  this  study  uses  Prolog,  Flavors  [18],  and  Lisp  as  in  [16,17]  because  of  the  authors'  belief  that 
such  a  multiple  programming  paradigm  is  very  well  suited  to  the  complex  nature  of  the  motion 
coordination  problem  for  the  ASV,  involving  a  constant  interaction  among  the  on-line  optimization  logic, 
the  vehicle  and  its  internal  objects,  and  the  numerical  routines.  In  what  follows,  Prolog  is  used  to  program 
the  first  part,  and  Flavors  and  Lisp  are  introduced  to  program  the  second  and  the  third  parts.  This  division 
of  the  motion  coordination  problem  is  suggested  by  the  physiology  of  animals  which  utilize  a  brain,  a 
physical  body,  and  muscles  to  move  themselves.  Moreover,  like  the  hierarchical  organization  existing 
among  the  these  functional  parts  of  animals,  the  program  described  in  this  report  is  also  organized  according 
to  same  type  of  hierarchy.  Specifically,  the  top  level  of  the  program  is  the  motion  coordinator  written  in 
Prolog,  while  the  second  level  is  the  Flavor  objects  which  simulate  a  body,  legs,  and  sensory  organs.  The 
numerical  function  routines  written  in  Lisp  perform  all  necessary  calculations  to  move  the  legs  and  the 
body  analogous  to  the  action  of  muscles.  The  resulting  code  is  remarkably  easy  to  understand  and  modify 
because  each  programming  language  naturally  provides  the  right  programming  paradigm  for  each  division  of 


the  program.  Moreover,  the  resulting  code  is  at  least  an  order  of  magnitude  shorter  than  the  corresponding 
Pascal  code  for  same  reason.  Consequently,  this  approach  significanUy  reduces  development  time. 

The  remainder  of  this  report  first  presents  definitions  used  in  this  report,  and  a  discussion  of  the 
mathematical-model  used  to  simulate  terrain  and  the  ASV  vehicle.  This  is  followed  by  a  description  of  the 
ditch  crossing  motion  coordination  rule-set,  and  the  use  of  Prolog  to  realize  both  ditch  crossing  motion 
coordination  and  normal  motion  coordination.  The  report  concludes  with  a  discussion  of  the  results  of  the 
investigation  and  suggestions  for  future  research. 


2.       Definitions 

In  this  report,  a  number  of  definitions  are  used  as  follows: 

Definition  1 :  A  foothold  is  a  point  on  a  segment  of  terrain,  and  can  be  assigned  to  a  leg  while  the 
leg  is  in  the  air.  When  the  foot  of  a  leg  is  placed  on  the  terrain,  its  assigned  foothold  becomes  the  support 
point  of  the  leg.  A  foothold  associated  with  a  leg  can  be  changed  to  a  new  one  before  the  foothold  becomes 
a  support  point  [10]. 

Definition  2:  The  support  pattern  associated  with  a  given  set  of  leg  support  points  is  the  convex 
hull  of  the  vertical  projections  of  all  support  points  into  a  horizontal  plane  [13]. 

Definition  3:  The  magnitude  of  the  stability  margin  at  time  t  for  an  arbitrary  support  pattern  is 
equal  to  the  shortest  distance  from  the  vertical  projection  of  the  vehicle  center  of  gravity  to  any  point  on  the 
boundary  of  the  support  pattern.  If  the  pattern  is  statically  stable,  the  stability  margin  is  positive. 
Otherwise,  it  is  not  defined  [15]. 


Definition  4:  A  working  volume  is  associated  with  each  leg.  This  volume  is  a  subset  of  three- 
dimensional  space  defined  relative  to  the  body  and  consists  of  the  collection  of  points  which  can  be  reached 
by  the  foot  of  the  given  leg  [11,19]. 

Definition  5:  A  temporal  kinematic  margin  is  associated  with  each  foothold.  At  any  instant,  this 
margin  is  the  time  remaining  until  the  associated  leg  would  reach  the  boundary  of  its  working  volume  if  the 
foothold  were  used  as  a  support  point  [15,19]. 


3.  Vehicle  and  Terrain  Model 

While  the  vehicle  model  used  in  this  study  is  based  on  the  ASV,  it  represents  only  the  major 
vehicle  dimensions  and  components.  Specifically,  the  cabin  and  the  terrain  scanner  are  omitted  from  the 
simulation  model,  while  the  geometries  of  the  body  and  the  legs  are  identical  to  those  of  the  ASV. 
Therefore,  the  simulation  model  is  represented  by  a  simple  six-faced  box  with  each  leg  drawn  as  two  line 
segments  as  shown  in  Figure  1.  The  exact  vehicle  dimensional  data  can  be  found  in  other  literature  [2,7]. 
Differing  from  the  most  previous  simulation  studies  related  to  gaits  and  control  of  stepping 
[10,1 1,15,16,19],  which  simply  ignore  the  overlapped  portions  of  adjacent  working  volumes,  in  this  study, 
the  overlapped  portions  are  taken  into  account  during  the  foothold  selection  process  in  order  to  utilize  the 
full  kinematic  capability  of  the  vehicle. 

The  terrain  adopted  for  this  study  is  made  up  of  terrain  cells,  and  these  individual  cells  are  classified 
into  two  types  of  cells.  One  type,  called  a  permitted  cell,  is  able  to  support  the  body  load  when  a  leg  steps 
on  it.  The  other  type,  named  a  forbidden  cell,  is  not  usable  because  of  unfavorable  terrain  conditions. 
Though  this  classification  is  complete  with  respect  to  individual  terrain  cells,  there  is  a  chance  that  a  group 
of  the  forbidden  cells  can  constitute  a  large  structured  obstacle  instead  of  being  randomly  distributed.  In  this 
study,  one  type  of  structured  large  obstacle,  a  ditch,  is  considered  because  of  its  special  shape,  a  long  length 
and  a  relatively  narrow  width.  Due  to  its  shape,  the  most  effective  way  to  overcome  a  ditch  is,  if  possible, 
crossing  it  instead  of  avoiding  it  by  going  around  it.  However,  avoidance  may  be  a  better  choice  for  other 


types  of  large  obstacles.  This  possibility  is  not  studied  in  this  report  Rather,  the  simulation  terrain  is 
ternary  terrain  which  is  composed  of  permitted  cells,  forbidden  cells,  and  a  ditch.    A  typical  terrain  example 
utilized  in  this  study  is  shown  in  Figure  1.  A  cell  with  an  "X"  mark  is  a  forbidden  cell  or  a  part  of  a  ditch 
area  while  unmarked  cells  are  permitted.  A  ditch  is  shown  in  the  middle  of  the  simulation  terrain. 
Forbidden  cells  on  this  terrain  can  be  designated  either  manually  by  an  operator  or  automatically  by  using  a 
random  number  generator  with  a  threshold  chosen  to  produce  a  specified  ratio  between  permitted  cells  and 
forbidden  cells  [11]. 

The  dimensions  of  each  cell  are  one  foot  by  one  foot.  This  size  is  comparable  to  that  of  the  feet  of 
the  ASV,  and  is  larger  than  the  resolution  of  the  terrain  scanner  [5,12]. 

An  overall  block  diagram  of  the  program  developed  in  this  study  is  shown  in  Figure  2.  This  entire 
program  is  written  for  a  Symbolics  3650  Lisp  machine  [1 1,20,21].  Each  box  shown  is  an  object  that  is  an 
instance  of  a  Flavor  [18]  with  the  exception  of  the  Free  Gait  Coordinator  which  is  written  in  Symbolics 
Prolog  [22].  Like  the  physical  ASV  which  has  nine  major  parts,  namely,  a  body,  a  vision  sensor,  a  cab, 
and  six  legs,  the  simulation  object,  "ASV"  has  correspondingly  nine  component  objects,  "Body",  "Vision 
Sensor",  "Joystick",  and  "Legl"  through  "Leg6".  These  nine  objects  are  linked  to  "ASV"  through  a  part 
relation  [16,23].  Each  part  has  its  sub-parts,  and  again  is  linked  to  them  with  a  part  relation.  Differing 
from  the  nine  major  parts  which  have  visible  corresponding  parts  in  the  real  ASV,  the  subparts  of  the 
simulation  are  not  physically  tangible,  but  are  introduced  because  of  their  functionalities  for  program 
development.  For  example,  the  "Legl"  object,  which  is  a  part  of  the  "ASV"  object,  has  six  subparts: 
"Legl  Plan  Machine",  "Legl  Control  Machine",  "Legl  Executor",  "Legl  Contact  Sensor",  "Legl  Foothold 
Finder",  and  "Legl  TKM  Calculator".  Through  the  use  of  a  make-instance  function  in  an  appropriate 
Flavor  slot,  the  "Legl"  object  binds  all  of  these  subparts  into  one  group  with  the  part  relation  [18].  In 
order  to  show  the  above  relations  among  the  objects  in  Figure  2,  the  six  subpart  objects  are  drawn  under  the 
"Legl"  object. 

Besides  the  part  relation,  Figure  2  also  shows  the  hierarchical  control  structure  linking  the 
simulation  objects.  Specifically,  communication  is  restricted  between  objects  in  two  adjacent  levels  by  an 
assumption  that  upper  levels  have  the  right  to  access  status  information  at  lower  levels,  but  the  latter  must 


receive  explicit  commands  from  upper  levels  to  update  their  internal  states.  For  example,  when  "ASV",  the 
vehicle  object,  needs  "Legl"  to  support  its  body,  it  sends  a  "Place"  decision  to  "Legl"  and  continuously 
monitors  "Legl"  as  to  whether  "Legl"  has  begun  to  support  the  body  or  is  in  motion  to  try  to  reach  a 
foothold.  On  receiving  a  "Place"  decision  from  "ASV",  "Legl"  sends  the  "Place"  decisions  to  "Legl  Plan 
Machine"  while  making  observations  of  this  machine.  This  type  of  message  passing  to  and  status 
observation  from  subordinates  continues  until  the  "Place"  decision  is  accomplished.  That  is,  when  the  foot 
of  "Legl"  actually  hits  the  ground,  the  contact  sensor  of  "Legl"  detects  the  event  and  changes  its  internal 
state.  The  state  change  of  "Legl  Contact  Sensor"  is  observed  by  "Legl  Executor"  and  by  "Legl  Control 
Machine".  In  this  way,  the  state  change  in  the  lowest  level  is  propagated  to  higher  levels  until  the  touch 
down  event  arrives  at  "ASV".  The  detailed  description  of  this  control  scheme  can  be  found  in  other 
literature  [16]. 

The  joystick  object  simulates  the  physical  three-axis  joystick  of  the  ASV  through  the  use  of  six 
keys  on  the  simulation  computer  keyboard  to  increment  or  decrement  each  of  the  three  rates  controlled  by 
the  joystick.  These  rates  art  forward  velocity,  lateral  velocity,  and  turn  rate,  all  in  body  coordinates.  The 
altitude  of  the  vehicle  above  the  terrain  and  its  orientation  in  roll  and  pitch  relative  to  the  terrain  are 
automatically  regulated  using  the  algorithms  described  in  [24]. 

While  an  elementary  representation  of  the  vision  sensor  is  included  in  the  program,  as  described  in 
the  above  discussion  of  terrain,  it  is  assumed  that  all  forbidden  cells  and  ditches  have  already  been  identified 
by  prior  terrain  analysis.  Of  course  this  assumption  does  not  represent  a  physical  limitation  of  the  ASV, 
but  as  made  merely  to  allow  this  simulation  to  be  focused  on  vehicle  control,  rather  than  on  vision. 

In  addition  to  simplification  of  vision,  this  simulation  also  ignores  leg  mass  in  order  to  avoid  the 
complexity  of  computing  a  center  of  gravity  which  moves  with  respect  to  the  body.  Moreover,  all  inertial 
forces  are  omitted  from  the  simulation.  That  is,  as  in  most  previous  simulation  studies  relating  to  gaits 
and  control  of  stepping  [8,9,10,11,16,19,25,26],  only  static  stability  is  considered  in  this  study.  While 
this  simplification  would  be  serious  in  high  speed  locomotion,  free  gaits  are  most  appropriate  to  low  speed 
traversal  of  extremely  difficult  terrain,  so  the  authors  do  not  feel  that  this  is  a  serious  limitation  on  the 
applicability  of  the  results  of  this  investigation. 


4.  Ditch    Crossing    Motion    Coordination 

When  the  vision  system  detects  the  existence  of  a  ditch,  the  vehicle  operation  mode  is  switched 
from  the  normal  free  gait  mode  [1 1,16]  to  the  ditch  crossing  mode.  In  contrast  to  the  normal  free  gait  mode 
which  performs  on-line  optimization  of  leg  stepping  under  an  environment  with  randomly  distributed  small 
obstacles  [11,16],  the  coordinator  in  the  ditch  crossing  mode  controls  the  vehicle  with  a  predetermined 
motion  sequence  in  order  to  effectively  overcome  a  ditch;  i.e.,  a  structured  large  obstacle.  The  ditch 
crossing  mode  is  composed  of  two  phases,  the  preparation  phase  and  the  main  phase,  and  these  two  phases 
are  sequentially  executed.  The  preparation  phase  provides  a  transition  period  from  the  normal  free  gait  mode 
to  the  ditch  crossing  mode,  and  the  main  phase  performs  the  actual  ditch  crossing  action. 

4.1  Preparation    Phase 

The  preparation  phase  is  composed  of  a  sequence  of  nine  states,  and  these  nine  states  are  grouped 
into  two  cycles  which  are  named  Cycle  1  and  Cycle  2.  The  first  cycle,  Cycle  1,  consists  of  six  states  and 
takes  care  of  a  transition  from  the  normal  mode  to  the  ditch  crossing  mode.  During  the  execution  of  the 
first  cycle,  the  body  attitude  is  modified  to  be  suitable  to  cross  a  ditch.  The  second  cycle,  Cycle  2,  consists 
of  three  states  and  causes  the  vehicle  to  have  the  correct  leg  configuration  for  the  ditch  crossing  operation  in 
the  main  phase.  Thus,  during  the  execution  of  Cycle  2,  the  body  is  not  moved  at  all.  The  graphical 
representation  of  the  preparation  phase  is  shown  in  Figure  3. 

The  six  states  in  Cycle  1  are  Place  Legs  in  the  Air,  Back  Middle  Legs,  Forward  Rear  Legs, 
Forward  Middle  Legs,  Forward  Front  Legs,  and  Lift  Middle  Legs  and  Move.  Body  movement  is  involved 
only  in  the  last  state.  The  first  state,  Place  Legs  in  the  Air,  represents  a  simple  action;  i.e.,  leg  placing, 
but  the  rest  of  the  states  represent  at  least  two  sequential  actions.  For  example,  during  the  Forward  Middle 
Legs  state,  the  middle  legs  are  lifted  from  the  ground  and  placed  on  the  ground  using  one  of  the  closest 
footholds  to  the  front  end  of  the  working  volumes  of  the  middle  legs.  Similarly,  in  the  Back  Middle  Legs 
state,  the  middle  legs  are  lifted  and  placed  at  one  of  the  closest  footholds  to  the  back  end  of  the  working 
volumes  of  the  middle  legs. 
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The  first  cycle  of  the  preparation  phase  begins  with  the  Place  Legs  in  the  Air  state.  During  this 
state,  all  the  legs  in  the  air  are  placed  on  the  ground  without  any  body  movement.  At  most  three  legs  will 
be  placed  in  this  state  because  at  least  three  legs  must  support  the  body  at  all  times  to  maintain  the  stability 
of  the  vehicle.  When  a  leg  is  placed  in  this  state,  one  of  the  closest  footholds  to  the  front  end  of  the 
working  volume  of  each  leg  is  selected  as  a  stepping  position  on  the  ground.  Thus,  the  newly  placed  legs 
have  larger  temporal  kinematic  margins  (TKMs)  than  the  legs  already  on  the  ground. 

At  the  end  of  the  first  state,  all  six  legs  are  on  the  ground.  Thus,  the  middle  legs  can  be  used  for 
any  purpose  because  the  front  and  the  rear  four  legs  are  sufficient  to  make  the  vehicle  stable  as  long  as  these 
four  legs  are  within  their  kinematic  limits.  Therefore,  the  middle  legs  are  used  to  provide  maximum  TKMs 
for  the  front  and  the  rear  legs  in  the  following  four  states. 

The  Back  Middle  Legs  state  is  the  first  state  of  the  sequence  to  maximize  TKMs  of  the  front  and 
the  rear  legs.  The  middle  legs  are  lifted  and  placed  at  the  back  ends  of  their  working  volumes.  At  the 
completion  of  this  state,  the  middle  legs  are  placed  behind  the  center  of  the  gravity  of  the  vehicle.  Thus, 
the  vehicle  can  maintain  its  stability  with  the  front  and  the  middle  legs  alone,  and  the  rear  legs  can  be  lifted. 

In  the  Forward  Rear  Legs  state,  the  rear  legs  are  lifted  and  placed  at  the  front  end  of  the  working 
volume  of  the  rear  legs.  Thus,  both  rear  legs  will  have  maximum  TKMs  at  the  end  of  this  state.  Though 
the  rear  leg  lifting  actions  are  inherently  safe,  before  lifting  one  of  the  rear  legs  the  vehicle  stability  is 
checked  to  ensure  that  the  vehicle  is  stable  without  the  rear  leg.  If  the  vehicle  is  not  stable,  the  leg  is  not 
lifted  from  the  ground,  and  the  ditch  crossing  operation  will  be  halted.  If  it  is  stable  without  the  leg,  the 
leg  is  lifted.  After  the  first  rear  leg  is  lifted  safely,  the  same  test  is  performed  on  the  other  rear  leg  before  it 
is  lifted.  This  type  of  test  is  always  performed  before  lifting  any  leg  from  the  ground  during  the  ditch 
crossing  operation  in  order  to  ensure  safe  operation.  At  the  end  of  the  Forward  Rear  Legs  state,  both  of  the 
rear  legs  are  placed.  Again,  the  middle  legs  become  redundant  for  the  vehicle  stability. 

In  the  Forward  Middle  Legs  state,  the  middle  legs  are  lifted  and  placed  at  the  front  end  of  the 
working  volumes  of  the  middle  legs.  Because  the  new  support  points  of  the  middle  legs  are  ahead  of  the 
center  of  the  gravity  of  the  vehicle,  the  vehicle  now  can  be  safely  supported  by  the  middle  and  the  rear  legs. 
Thus,  the  front  legs  can  be  lifted  from  the  ground  without  harming  the  stability  of  the  vehicle. 


In  the  Forward  Front  Legs  state,  the  front  legs  are  lifted  and  placed  at  the  edge  of  the  vehicle  side  of 
the  ditch.  This  has  to  be  done  because  the  new  support  points  of  the  front  legs  will  be  the  last  ones  on  the 
vehicle  side  of  the  ditch.  This  requirement  is  not  hard  to  meet  since,  as  long  as  the  edge  of  the  ditch  is 
included  in  the  working  volume  of  the  front  legs,  the  front  legs  can  always  be  put  in  the  right  position. 
However,  the  opposite  side  of  the  edge  of  the  ditch  should  not  be  included  in  the  working  volume  in  order 
to  prevent  its  being  used  as  possible  footholds  for  the  front  legs.  Therefore,  there  is  a  range  of  the  vehicle 
locations  with  respect  to  the  near  edge  of  the  ditch  so  that  the  vehicle  can  select  the  right  stepping  positions 
for  the  front  legs.  This  range  is  shown  in  the  following  equation: 

1                                                 1 
P  +  yL-DW<  DCIR  <P+yL  (1) 

where     DCIR     :  Ditch  Crossing  Initiation  Range 

with  respect  to  gravity  center  of  the  vehicle 

P  :  Pitch  between  adjacent  legs 

L  :  Longitudinal  length  of 

working  volume  for  each  legs 

DW        :  Ditch  Width. 

To  understand  this  relationship,  it  should  be  recognized  that  the  distance  from  the  vehicle's  center  to  the 
front  ends  of  the  working  volumes  of  the  front  legs  is  the  sum  of  the  pitch  between  the  middle  and  the  front 
legs  and  half  of  the  longitudinal  length  of  the  working  volume  of  the  front  legs.  Thus,  the  meaning  of 
Eq.  (1)  is  that  when  the  ditch  crossing  operation  is  initiated,  the  front  ends  of  the  working  volumes  of  the 
front  legs  should  be  positioned  between  the  near  edge  and  the  far  edge  of  the  ditch.  Evidently,  if  ditch 
crossing  is  initiated  anywhere  in  the  above  range,  the  ditch  crossing  operation  will  not  be  hampered  since 
the  body  attitude  and  the  leg  stepping  positions  are  corrected  during  the  preparation  phase.  It  should  be  also 
noted,  however,  that  if  DW  is  less  than  3  ft,  the  vision  system  does  not  have  to  detect  the  existence  of  the 
ditch  at  all.  The  normal  plan  developed  in  [16]  is  capable  of  handling  such  ditch  width  without  any 
problem. 

At  the  completion  of  the  Forward  Front  Legs  state,  the  front  legs  will  be  placed  on  the  vehicle 
side  edge  of  the  ditch.  Again,  the  middle  legs  become  redundant. 
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In  the  Lift  Middle  Legs  and  Move  state,  the  middle  legs  are  lifted  and  the  body  is  moved  into  the 
ditch  area  until  at  least  one  of  the  supporting  legs  (the  front  and  the  rear  legs)  reaches  its  kinematic  limit.  If 
too  many  obstacles  have  not  interfered  with  the  operations  of  the  previous  four  states,  there  will  be  a  high 
probability  for  the  front  legs  to  reach  their  kinematic  limits  first  because  footholds  in  the  front  most 
portion  of  the  working  volumes  of  the  front  legs  may  be  excluded  by  the  location  of  the  ditch.  Thus,  at  the 
end  of  this  state,  the  vehicle  body  is  fully  pushed  into  the  ditch  area  under  the  constraints  of  the  current  leg 
configuration,  and  its  movement  is  stopped.  At  this  point,  although  the  body  position  and  the  front  leg 
positions  are  right  for  the  ditch  crossing,  the  rear  leg  positions  are  not  appropriate  because  they  are  already 
near  their  kinematic  limits.  Thus,  further  body  movement  is  very  limited  or  impossib»   depending  on  the 
kinematic  margins  of  the  rear  legs.  Even  though  the  opposite  side  of  the  edge  may  not  be  reachable  by  the 
front  legs  at  the  end  of  the  Lift  Middle  Legs  and  Move  state,  the  vehicle  can  cross  the  ditch  if  it  moves 
further  into  the  ditch  area  by  eliminating  the  kinematic  problem  of  the  rear  legs  and  if  the  ditch  width  is 
narrower  than  the  vehicle  ditch  crossing  capability.    In  the  second  cycle  of  the  preparation  phase,  the 
kinematic  limits  of  the  rear  legs  are  eliminated. 

The  second  cycle  of  the  preparation  phase  has  three  states  and  starts  with  the  Back  Middle  Legs 
state.  No  body  movement  is  involved  in  the  second  cycle,  but  the  stepping  positions  of  the  rear  and  the 
middle  legs  are  rearranged. 

In  the  first  state,  the  middle  legs,  which  are  redundant  to  make  the  vehicle  stable,  are  lifted  and 
placed  near  the  back  end  of  their  working  volumes.  Because  the  new  support  points  of  the  middle  legs  are 
behind  the  center  of  the  gravity  of  the  vehicle,  the  rear  legs  can  be  lifted  without  harming  the  vehicle 
stability. 

In  the  Forward  Rear  Legs  state,  the  rear  legs  are  lifted  and  placed  near  the  front  end  of  their  working 
volumes.  Thus,  the  rear  legs  obtain  maximum  kinematic  margins.  At  this  point,  though  the  rear  legs 
provide  maximum  body  movement  potential,  the  middle  legs  prohibit  further  body  movement. 

In  the  Forward  Middle  Legs  state,  the  middle  leg  kinematic  problem  is  eliminated.  The  middle 
legs  are  lifted  and  placed  at  the  front  end  of  the  working  volume  of  the  middle  legs.  Therefore,  both  the 
middle  and  the  rear  legs  have  their  maximum  kinematic  margins,  while  the  body  is  completely  pushed  into 
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the  ditch  area.  In  contrast  to  the  middle  and  the  rear  legs,  the  front  legs  should  be  at  their  kinematic  limits 
because  the  front  legs  stepping  positions  have  not  changed  since  they  were  at  their  kinematic  limits  at  the 
end  of  Cycle  1.    If  the  front  legs  are  now  lifted,  vehicle  body  movement  can  be  resumed.  This  will  be  the 
first  action  of  the  following  phase,  the  Main  Phase. 

In  summary,  at  the  end  of  the  preparation  phase,  the  body  is  fully  moved  into  the  ditch  area  within 
the  limits  of  the  stability  of  the  vehicle  and  the  kinematics  of  the  legs.  The  middle  legs  and  the  rear  legs 
are  fully  forward  to  enhance  the  ditch  crossing  capability,  and  the  front  legs  are  ready  to  be  lifted  from  the 
ground.  This  preparation  allows  the  vehicle  to  cross  a  wider  ditch  than  the  longitudinal  length  of  the 
vehicle  legs'  working  volumes.  Though  the  other  side  of  the  ditch  is  not  included  in  the  front  legs'  working 
volumes  at  the  end  of  the  preparation  phase,  the  body  can  move  forward  as  long  as  the  vehicle's  stability  is 
maintained  and  the  leg  kinematic  limits  are  not  reached.  That  is,  if  the  other  side  of  the  ditch  is  included  in 
the  front  leg's  working  volume  before  the  vehicle  becomes  unstable  and  before  the  other  legs  reach  their 
kinematic  limits,  the  other  side  is  reachable  by  the  front  legs.  If  the  front  legs  can  be  placed  on  the  other 
side  within  the  vehicle's  kinematic  and  stability  limitations,  then  the  vehicle  can  cross  the  ditch  because  the 
ASV  legs'  working  volumes  are  identical  and  because  the  pitches  between  the  front  legs  and  the  middle  legs 
and  between  the  middle  legs  and  the  rear  legs  are  the  same. 

As  a  result  of  the  above  arguments,  the  maximum  ditch  width  can  be  crossed  by  the  ASV,  which 
has  identical  working  volumes  for  all  legs  and  the  equal  pitches  between  the  front  and  the  middle  legs  and 
between  the  middle  and  the  rear  legs,  is  determined  by  both  the  pitch  length  and  the  length  of  the 
longitudinal  working  volumes  of  the  legs.  Specifically,  the  maximum  ditch  width  can  be  crossed  by  the 
ASV  is  given  by: 

MDW  =  P+    yL-SM-SDE  (2) 

where     MDW     :  Maximum  Ditch  Width 

P  :  Pitch  between  adjacent  legs 

L  :  Longitudinal  length  of 

working  volume 
SM        :  Safety  Margin 
SDE       :  Search  Digitization  Effect. 
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As  can  be  seen,  the  maximum  ditch  width  (MDW)  is  calculated  by  adding  the  pitch  between  adjacent  legs 
and  the  half  of  the  longitudinal  length  of  the  working  volumes  of  the  legs,  and  then  by  subtracting  the 
safety  margin  (SM)  and  the  search  digitization  effect  (SDE).  The  safety  margin  is  a  prescribed  margin 
ensuring  safe  operation  of  the  vehicle.  The  search  digitization  effect  is  an  artifact  of  the  foothold  search 
process  resulting  from  a  one  foot  by  one  foot  grid  search.  With  the  dimensions  of  the  simulation  model 
discussed  in  [2,7],  the  MDW  becomes  8.5  ft  when  the  SM  and  the  SDE  are  0.5  ft,  respectively. 

4.2     Main  Phase 

The  Main  Phase  is  composed  of  three  cycles  The  first  and  the  third  cycles  are  composed  of  three 
states  each,  while  the  second  cycle  contains  only  one  state.  The  first  cycle  in  the  main  phase  in  the 
program  is  named  Cycle  3  to  show  continuation  from  the  preparation  cycles.  Consequently,  Cycle  1  and 
Cycle  2  belong  to  the  Preparation  Phase,  and  Cycle  3  through  Cycle  5  belong  to  the  Main  Phase.    A 
graphical  representation  of  the  Main  Phase  is  shown  in  Figure  4. 

The  first  cycle,  Cycle  3  is  composed  of  three  states,  Move  Forward  Front  Legs,  Move  Back  Middle 
Legs,  and  Move  Forward  Rear  Legs.    During  this  cycle,  the  front  legs  cross  the  ditch,  and  the  rear  legs  are 
prepared  to  replace  the  middle  legs  which  will  cross  the  ditch  in  the  following  cycle.  In  this  cycle,  the 
vehicle  body  is  allowed  to  move  forward  whenever  possible.  Therefore,  all  the  state  names  are  pre-fixed 
with  "Move",  and  each  state  is  composed  of  three  actions,  leg  lifting,  body  movement,  and  leg  placement. 

In  the  Move  Forward  Front  Legs  state,  first,  the  front  legs  are  lifted  while  the  body  is  not  moved. 
As  soon  as  both  front  legs  are  lifted  from  the  ground,  the  second  action  is  performed,  which  is  a  forward 
body  movement.  This  body  movement  is  sustained  until  the  middle  legs  limit  this  movement  because  the 
middle  leg  positions  with  respect  to  the  center  of  the  vehicle  gravity  determine  the  stability  margin  when 
only  the  middle  and  the  rear  legs  support  the  body.  Though  the  middle  legs  can  kinematically  move  behind 
the  center  of  gravity,  they  should  be  stopped  in  front  of  the  center  of  gravity  to  maintain  the  safety  stability 
margin. 

When  the  body  movement  is  stopped  with  the  completion  of  the  second  action  of  the  current  state, 
the  opposite  side  of  the  ditch  will  be  included  in  the  working  volumes  of  the  front  legs  if  the  width  of  the 
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ditch  is  narrower  than  MDW.  Thus,  the  third  action  of  the  Move  Forward  Front  Legs  state  follows,  in 
which  the  front  legs  are  placed  on  the  opposite  side  of  the  ditch.  Thus,  the  middle  legs  become  redundant 
for  the  vehicle  stability. 

In  the  second  state  of  Cycle  3,  the  Move  Back  Middle  Legs  state,  the  middle  legs  are  lifted  from 
the  ground.  The  vehicle  body  movement  is  resumed  because  the  movement  is  restricted  by  the  middle  legs 
to  maintain  the  vehicle  stability  margin.  The  body  movement  is  continued  until  any  one  of  the  supporting 
legs  meets  its  kinematic  limit.  Specifically,  the  body  will  be  moved  until  one  or  both  rear  legs  reach  its  or 
their  kinematic  limits  because  the  kinematic  margins  of  the  rear  legs  have  been  used  to  move  the  body  in 
the  previous  state,  but  those  of  the  front  legs  have  been  just  maximized  in  the  previous  state,  the  Move 
Forward  Front  Legs  state  of  Cycle  3.  This  effect  can  be  easily  seen  in  the  second  drawing  of  Cycle  3 .State 
2  in  Figure  4,  and  "Rear  Legs"  written  on  the  top  of  the  second  drawing  shows  the  termination  condition  of 
the  current  body  movement.  When  the  kinematic  limits  of  the  rear  legs  stop  the  body  movement,  the  third 
action  of  the  current  state  is  performed,  which  is  to  place  the  middle  legs  at  the  back  end  of  their  working 
volumes.  Because  the  middle  legs  are  placed  behind  the  center  of  gravity,  the  rear  legs  can  be  lifted  from  the 
ground  while  the  front  and  the  middle  legs  stably  support  the  body. 

In  the  Move  Forward  Rear  Legs  state,  which  is  the  third  state  of  Cycle3,  first,  the  rear  legs  are 
lifted  from  the  ground.  As  soon  as  the  rear  legs  are  lifted,  the  body  movement  is  resumed.  However,  the 
body  movement  is  immediately  blocked  by  the  middle  legs  which  have  placed  back  in  the  previous  state. 
Thus,  the  third  action  of  the  current  state,  which  places  the  rear  legs  at  the  front  end  of  their  working 
volumes,  is  immediately  started.  Consequently,  very  little  body  movement  is  involved  in  this  state,  but 
the  rear  legs  gain  large  TKMs  so  that  the  body  can  be  moved  further  in  the  next  cycle.  Therefore,  though 
the  other  side  of  the  ditch  may  not  reachable  by  the  middle  legs  under  the  current  body  position,  the  new 
body  movement,  which  will  be  performed  in  the  next  cycle,  will  make  this  possible. 

The  second  cycle  of  the  main  phase,  Cycle  4,  is  composed  of  one  state,  the  Move  Forward  Middle 
Legs  state.  In  this  cycle,  the  middle  legs  will  be  moved  to  the  other  side  of  the  ditch.  The  first  action  is 
lifting  the  middle  legs  so  that  the  body  movement  can  be  resumed.  This  movement  will  last  as  long  as  the 
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front  legs  have  positive  TKMs  because  the  TKMs  of  the  front  legs  have  already  been  partially  consumed  in 
the  previous  cycle.  This  is  shown  in  the  second  drawing  of  Cycle  4:State  1  in  Figure  4. 

When  the  body  movement  is  stopped,  the  other  side  of  the  ditch  is  reachable  by  the  middle  legs 
because  the  geometries  of  the  front  and  the  middle  legs  are  identical  and  because  the  working  volumes  of  the 
front  and  the  middle  legs  overlap  slightly  at  the  rear  end  of  the  former  volume  and  the  front  end  of  the  latter 
volume.  As  soon  as  the  middle  legs  are  positioned  on  the  other  side  of  the  ditch,  this  cycle  is  terminated. 

The  last  cycle,  Cycle  5,  which  is  the  third  cycle  of  the  main  phase,  takes  care  of  the  ditch  crossing 
action  of  the  rear  legs.  This  cycle  is  composed  of  the  three  states,  the  Move  Forward  Front  Legs  state,  the 
Move  Back  Middle  Legs  state,  and  the  Move  Forward  Rear  Legs  state.  These  three  states  are  the  same  that 
of  Cycle  3.   This  is  not  a  coincidence,  but  an  expected  consequence  of  the  geometrical  symmetry  of  the 
front  and  the  rear  legs. 

In  the  Move  Forward  Front  Legs  states,  the  kinematic  problems  of  the  front  legs  which  block 
further  body  movement  are  relieved  because  the  first  action  of  this  state  is  to  lift  the  front  legs  from  the 
ground.  Thus,  the  body  movement,  which  is  the  second  action  of  this  state,  is  resumed,  and  is  terminated 
by  the  positions  of  the  middle  legs  with  respect  to  the  body  because  the  positions  of  the  middle  legs 
determine  the  stability  of  the  vehicle.  When  this  state  is  terminated,  the  front  legs  are  placed  on  the  ground. 

In  the  Move  Back  Middle  Legs  state,  the  body  movement  is  resumed  as  soon  as  the  middle  legs  are 
lifted  from  the  ground.  This  movement  will  last  until  the  kinematic  limits  of  the  rear  legs  are  reached. 
When  this  condition  is  met,  the  body  movement  is  stopped  and  the  middle  legs  are  placed  as  far  backward  as 
possible  so  that  the  middle  legs  can  support  the  body  together  with  the  front  legs.  Consequently,  the 
middle  legs  will  be  placed  at  the  edge  of  the  ditch  because  this  edge  is  in  the  working  volumes  of  the  middle 
legs. 

The  Move  Forward  Rear  Legs  state,  which  is  the  last  state  in  the  last  cycle,  causes  the  rear  legs  to 
cross  the  ditch.  The  first  action  is  to  lift  the  rear  legs  from  the  ground.  Body  movement  is  then  resumed 
and  continued  until  the  other  side  of  the  ditch  is  reachable  by  the  rear  legs.  Again,  this  will  be 
accomplished  because  the  working  volumes  of  the  middle  and  the  rear  legs  slightly  overlap.  Finally,  all  the 
legs  are  across  the  ditch.  Thus,  the  ditch  crossing  operation  has  accomplished  and  the  operational  mode  is 
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switched  back  to  the  normal  mode,  and  the  Free  Gait  Motion  Coordinator  [16]  regains  control  of  the 
vehicle. 


5.   Program   Implementation 

The  top  level  Ternary  Terrain  Motion  Coordinator  is  written  in  Symbolics  Prolog  because  of  its 
easy  translation  characteristics  from  natural  language  to  a  computer  program,  and  because  of  its 
straightforward  interface  to  Symbolics  Lisp  language  in  which  the  rest  of  the  program  is  written.  The 
Prolog  program  is  listed  in  Figure  5.  It  is  composed  of  three  functional  groups  of  predicates.  The  first 
group  controls  the  flow  of  the  whole  program,  while  the  second  does  logic  processing  which  generates 
commands  for  the  vehicle  body  and  legs.  The  last  group  is  responsible  for  bridging  between  the  program 
written  in  Prolog  and  the  robot  program  in  Flavor  objects.  This  is  accomplished  through  the  Lisp  function 
call  facility  provided  by  Symbolics  Prolog.  Specifically,  anything  following  the  "is"  predicate  in  a  Prolog 
clause  may  be  either  a  Prolog  arithmetic  function  or  the  name  of  a  Lisp  function  [22].  If  a  Lisp  function 
name  follows  the  "is"  predicate,  it  is  evaluated  according  to  its  definition  inside  the  Lisp  environment.  In 
the  program  of  Figure  5,  arguments  following  "is"  predicates  are  names  of  Lisp  functions,  and  make 
connections  to  the  Lisp  environment.  A  returned  value  resulting  from  a  Lisp  function  call  may  be  used  to 
instantiate  a  variable  preceding  the  "is"  Prolog  predicate  or  test  whether  the  returned  value  matches  a  value 
preceding  the  "is"  predicate.  In  the  former  case,  the  subgoal  "is"  always  succeeds,  but  the  latter  case,  only 
when  two  values  agree  does  the  "is"  subgoal  succeed.  In  the  program,  only  the  former  case  is  used.  The 
Lisp  portion  of  the  program  is  listed  in  the  appendix  attached  at  the  back  of  this  report. 

The  Prolog  program  is  started  by  typing  "robot"  on  the  computer  console.  The  robot  clause  is  the 
first  line  of  the  program.  After  the  initialization  process  is  done,  it  makes  the  loop  clause  repeat  Thus,  it 
determines  the  flow  of  the  whole  program. 

The  loop  clause  is  composed  of  three  subgoals,  get_command,  plan,  and  execute.  This  shows  the 
flow  of  the  program  execution  for  each  loop.  Based  on  the  input  command  from  the  joystick,  motion  is 
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planned,  and  the  planned  motion  is  executed.  Then,  the  executed  motion  is  drawn  on  the  screen  by  the 
draw_robot  clause  which  actually  calls  a  corresponding  Lisp  function,  graphical  display. 

The  plan  subgoal  of  the  loop  clause  has  two  alternatives,  free_gaits_motion_coordination_plan  and 
ditch_crossing_motion_coordination_plan.  In  the  Prolog  program,  the  ditch  mode  subgoal  is  tested  first 
because  ditch _crossing_motion coordination _plan  deals  with  a  more  specific  case  than  the  other.  If  the 
ditchjnode  subgoal  succeeds,  then  ditch_crossing_motion_coordination_plan  is  executed.  If  not,  then 
free  jgaitsjnotion  coordination _plan  is  executed.  The  free_gaits_motion_coordination_plan  clause  is 
composed  of  update jobot _state,  checkjkmjimit,  leg_plan,  body_plan,  and  generate  decision  subgoals. 
The  first  and  the  second  subgoals  update  the  state  and  the  body  position  of  the  vehicle  *»  i  check  kinematic 
problems  of  the  legs.  The  third  subgoal,  leg_plan,  performs  on-line  optimization  for  leg  coordination 
using  the  free  gait  strategy  [11,16].  Based  on  the  leg  plan,  the  fourth  subgoal,  body_plan,  plans  the  body 
movement  to  enhance  the  vehicle  stability.  Finally,  the  generate _decision,  subgoal  sends  decisions  to  the 
"ASV"  robot  flavor  object.  A  detailed  description  of  \hzfree_gaits_motion_coordination_plan  can  be  found 
in  other  literature  [11,16]. 

The  ditch _crossing_motion coordination _plan  and  the  related  clauses  implement  the  ditch  crossing 
coordination  discussed  in  the  previous  section.  There  are  two  ditch_crossing_motion_coordination_plan 
clauses  in  the  program,  and  the  first  clause  checks  the  termination  condition  while  the  second  clause 
performs  the  ditch  crossing  planning.  If  the  ditch  crossing  activity  is  not  completed,  then  the  first  clause 
fails,  and  the  second  ditch_crossing_motion_coordination_plan  clause  is  executed.  Thus,  the  cycle  jplanner 
clause  is  called  into  an  action. 

The  cycle _planner  clause  and  related  clauses  follow  the  above  two  ditch_plan  clauses.  This  group 
of  clauses  is  named  "Cycle  Planner".  The  first  clause  of  the  "Cycle  Planner"  group,  ditch _plan_done, 
checks  the  completion  of  the  ditch  crossing  plan.  The  two  cycle  jplanner  clauses  take  care  of  ditch  plan 
cycle  changes  from  cycle  1  to  cycle  5  by  increasing  the  cycle  number  whenever  one  cycle  is  completed. 
Therefore,  the  ditch _plan_done  clause  succeeds  as  soon  as  cycle  5  is  finished  because  the  cycle  number 
becomes  6  immediately  after  the  completion  of  Cycle  5.  The  last  subgoal  of  ditch _plan  done,  is 
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idlecycle,  which  is  a  dummy  plan  without  any  leg  planning.  It  is  introduced  to  fill  the  gap  for  the 
transition  between  cycles. 

The  subgoals  used  in  the  cycle _planner  clauses,  one_cycle_done  and  plan_cycle,  are  grouped  in  the 
following  part  of  the  program,  which  is  called  the  "Plan  Cycle  Dispatcher"  group.  The  one _cycle _done 
clause  checks  the  termination  condition  of  one  plan  cycle,  and  the  five  plan_cycle  clauses  execute  the 
appropriate  ditch  plan  cycles  based  on  the  plan  cycle  number  which  is  given  through  the  plan_cycle  fact  in 
the  Prolog  data  base. 

The  ditch_plan_cycle  clauses,  ditch  _plan_cy  del  through  ditch  _plan_cycle5 ,  form  another  group 
called  "Cycles"  in  the  program  following  the  "Plan  Cycle  Dispatcher"  group.  The  first  subgroup, 
ditch  _plan_cy  del ,  has  seven  clauses.  The  first  clause  of  this  subgroup  takes  care  of  the  initial  state 
transition,  and  the  rest  of  them  represent  the  six  states  in  cycle  1  discussed  in  the  previous  section. 
Specifically,  the  first  clause  retracts  plan _state( start),  which  is  a  cycle  starting  fact,  from  the  Prolog  data  base 
and  asserts  a  new  fact,  planstatefplace  Jegsjnjhe  _air),  which  is  the  name  of  the  first  state.  After  changing 
the  Prolog  data  base,  the  first  clause  executes  the  place  Jegsjnjhe  _air  subgoal,  which  performs  the  place 
legs  in  the  air  state  in  Cycle  J.  When  the  place  legs  in  the  air  subgoal  is  executed,  the  first  clause 
provides  the  next  state  information  for  the  subgoal  so  that  when  the  current  subgoal  is  completed  the  correct 
information  about  the  succeeding  state  is  asserted  in  the  data  base.  The  second  clause  through  the  seventh 
clause  sequentially  represent  six  states  in  cycle  1.  Thus,  these  ordered  clauses  represent  the  sequence  of  state 
transitions  among  the  six  states.  When  the  last  clause  calls  the  lift  middle  legs  and  move  subgoal, 
one _plan  cycle _done  is  given  instead  of  the  name  of  the  next  state  to  assert  the  cycle  termination  fact  in  the 
data  base.  This  structure  is  repeated  for  the  rest  subgroups  of  the  "Cycles"  group,  ditch _plan_cycle2  through 
ditch_plan_cycle5. 

The  following  group  called  "States"  is  composed  of  10  different  subgroups,  and  each  subgroup  is 
composed  of  two  clauses.  These  clauses  accept  information  about  the  next  state  so  that  the  next  state 
information  is  asserted  when  the  current  state  is  completed.  However,  only  the  first  clause,  which  takes 
care  of  its  state  transition,  utilizes  the  next  state  information.  The  second  clause  ignores  the  next  state 
information  and  executes  a  subgoal  whose  name  is  its  clause  head  name  pre-fixed  with  "do_".  Additionally, 
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both  clauses  of  each  subgroup  determine  the  body  movement  by  executing  either  the  stop  or  the  move 
subgoal  depending  on  the  needs  of  each  state  described  in  the  previous  section. 

The  "State  Executors"  clause  group  follows  the  "States"  clause  group.  This  clause  group  is 
composed  of  1 1  subgroups  of  clauses.  Among  them,  10  subgroups  are  responsible  for  execution  of  10 
states  in  the  "States"  group,  while  the  eleventh  subgroup  takes  care  of  the  body  movement,  such  as  move, 
stop,  clear  jnove  memory ,  and  move  done.  Therefore,  except  for  the  last  subgroup,  each  subgroup  shows  a 
sequence  of  actions  within  a  state,  which  are  described  in  the  previous  section.  For  example,  the 
do  jback jniddle  Jegs  clause  subgroup,  which  is  the  first  subgroup  of  the  "State  Executors"  group,  is  started 
with  three  major  clauses.  The  first  clause,  backjniddlejegsjione,  tests  the  state  termination  condition,  and 
the  second  and  the  third  clauses  perform  a  sequence  of  actions,  which  are  lifting  the  middle  legs  and  then 
placing  them  at  the  back  side  of  their  reachable  areas.  The  second  clause  tests  whether  both  middle  legs  are 
lifted  by  executing  the  subgoal,  all  middle  legs  lifted.  Initially,  this  test  should  fail.  Thus,  the  third  clause 
is  executed.  After  the  third  clause  is  executed  twice,  the  both  middle  legs  will  have  been  lifted  from  the 
ground  because  the  lift  jniddle  _legs  subgoal  causes  one  middle  leg  to  be  lifted  from  the  ground  at  a  time.  In 
the  do jback  middle  legs  subgroup,  there  are  two  lift  middle _legs  clauses.  The  first  clause  performs  the  leg 
lifting  action  by  selecting  one  middle  leg  and  then  causing  it  to  be  lifted  from  the  ground,  while  the  second 
clause  performs  a  default  action  by  always  succeeding.  Only  when  the  both  middle  legs  are  lifted  from  the 
ground,  is  the  middle  leg  placement  executed.  Specifically,  when  the  all  jniddle  Jegsjif ted  subgoal  in  the 
second  clause  of  this  subgroup  is  satisfied,  the  place  middle  Jegsjback  subgoal  is  executed.  If  the  middle 
legs  are  placed  on  the  ground  again,  men  the  backjniddlejegsjione  clause,  which  is  the  first  clause  of  the 
current  subgroup,  succeeds  because  the  all  jniddle  Jegsjif  ted  subgoal  has  been  satisfied  by  the 
middle  Jegs(lifted)  fact  which  was  asserted  when  middle  legs  were  lifted  from  the  ground,  and  because  the 
all  middle  legsjplaced  subgoal  is  now  satisfied.  Before  completing  the  backjniddlejegsjione  clause,  the 
first  clause  of  the  current  subgroup,  the  clear  middle  Jif ted jnemory  and  the  clear  jnove  jnemory  subgoals 
clear  residual  facts  generated  during  execution  of  the  do  jback jniddle  Jegs  clauses  in  order  not  to  interfere 
with  the  execution  of  the  following  "State  Executions"  subgroup  clauses. 
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Rest  of  the  subgroups  in  the  "State  Executors"  group  have  the  exactly  same  structure  that  of  the 
do  back jniddle  legs  clause  subgroup.  Specifically,  one  state  termination  clause  is  followed  by  two  state 
execution  clauses  which  are  pre-fixed  with  "do_"  and  related  clauses  which  support  these  leading  three 
clauses.   If  the  related  clauses  are  already  available,  they  are  not  duplicated  by  adding  them  in  the  subgroup. 

The  only  exception  to  the  above  structure  is  the  fifth  subgroup,  the  do _lift jniddle _legs  clause 
subgroup.  This  subgroup  is  composed  of  one  clause,  and  there  is  no  clause  to  test  the  state  termination 
condition.  The  time  required  to  complete  the  middle  leg  lifting  action  in  the  lift  jniddle _legs_andjnove 
state,  which  is  the  only  state  that  utilizes  the  do _lift  jniddle _legs  clause,  is  considerably  shorter  than  that 
needed  to  complete  the  body  movement  in  the  state.  Thus,  the  leg  lifting  action  is  always  guaranteed  before 
the  current  state  is  terminated. 

The  last  group  of  clauses  is  named  "Plan  Libraries".  These  clauses  are  used  by  both 
ditch_crossingjnotion_coordinationj)lan  and  freejgaitsjnotionjoordination  jplan.  This  group  is 
composed  of  two  subgroups,  body  jplan  and  generate  decision.  The  latter  subgroup  sends  planned  leg 
motions  through  decisions  to  the  robot,  "ASV",  which  is  a  flavor  object.  It  sends  them  one  by  one  unul 
all  the  decisions  in  the  Prolog  data  base  are  exhausted.  The  former  subgroup  takes  care  of  body  movement 
by  executing  speed  jplan  and  trajectory  jplan.  The  speed jjlan  clauses  control  the  speed  of  body  movement 
and  the  trajectory  jplan  clauses  modify  body  movement  trajectory  in  order  to  increase  the  stability  margin  of 
the  vehicle  using  a  "push"  operation  which  causes  the  gravity  center  of  the  vehicle  to  move  away  from  the 
boundary  of  the  current  supporting  pattern  [16]. 


6.  Discussion 

Performance  tests  were  carried  out  for  various  terrain  conditions  by  making  the  ASV  follow  a 
prescribed  standard  trajectory.  The  standard  trajectory  is  a  straight  line  across  the  model  terrain  from  one 
side  to  the  other  side  while  crossing  a  ditch  oriented  perpendicular  to  the  direction  of  vehicle  motion.  No 
failures  to  complete  the  standard  trajectory  were  observed  for  any  terrain  containing  up  to  a  8  ft  width  ditch 
if  no  randomly  distributed  forbidden  cells  were  included  in  the  terrain.  However,  when  forbidden  cells  were 
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added  to  the  terrain  with  a  8  ft  width  ditch,  the  performance  was  severely  degraded.  If  the  randomly 
distributed  forbidden  cells  occupied  30  percent  or  more  of  the  area  of  the  non-ditch  portion  of  the  whole 
simulation  terrain,  the  ASV  always  failed  to  complete  the  standard  trajectory.  Specifically,  the  ditch 
crossing  operation  was  halted  because  the  random  obstacles  on  the  ground  prevented  the  ASV  from  using 
the  most  favorable  stepping  positions  near  the  ditch.  However,  when  the  width  of  a  ditch  was  reduced  to 
7  ft,  no  failures  in  ditch  crossing  operations  were  observed.  Rather,  the  capability  to  overcome  randomly 
distributed  forbidden  cells  became  the  bottle  neck  which  determined  whether  the  ASV  could  complete  the 
standard  trajectory  or  not.  Overall,  when  less  than  70  percent  of  the  total  terrain  cells  were  the  forbidden 
cells,  the  program  made  the  ASV  follow  the  standard  trajectory  without  great  difficulties. 

One  of  the  advantages  of  using  object-oriented  programming  for  the  ASV  object  and  its  subobjects 
was  the  easy  extension  to  a  new  ASV  with  additional  functionality  required  for  the  ditch  crossing 
maneuvers.  Specifically,  this  was  accomplished  by  using  the  inheritance  mechanism  provided  by  Flavors. 
The  original  ASV  was  an  instance  of  "robot"  class,  and  the  new  extended  ASV  is  an  instance  of  "ditch- 
robot"  class.  The  latter  class  is  defined  as  a  subclass  of  the  former  class.  Thus,  the  entire  functionality  of 
the  "robot"  class  became  available  to  the  "ditch-robot"  class  through  the  inheritance  mechanism.  The  newly 
required  capabilities  were  added  to  the  "ditch-robot"  class  using  "defmethod"  which  defines  the  functionality 
of  a  class  in  Flavors.  The  result  was  remarkable.  The  additional  code  written  for  the  new  "ditch-robot" 
class  was  less  than  10%  of  the  size  of  the  original  "robot"  class,  and  roughly  more  than  95%  of  the  original 
code  was  reused. 

One  of  advantages  of  rule-based  control  of  motion  coordination  is  the  ease  of  extension  of 
coordination  logic  resulting  from  the  fact  that  individual  rules  or  a  group  of  rules  define  an  independent 
piece  of  behavior.  Instead  of  rewriting  all  the  code  related  to  motion  coordination,  the  new  ditch  crossing 
coordinator  was  simply  added  to  the  original  Prolog  code.  In  order  to  accept  the  new  coordinator,  the 
original  plan  Prolog  goal  in  [16]  was  subdivided  into  two  plan  subgoals,  free _gaits  motion  _coordi- 
nation_plan  and  ditch  crossing jnotion  coordination _plan.  The  old  plan  code  in  [16]  was  merely  renamed 
as  free _gaits  motion  coordination  _plan  without  any  further  modification,  and  the  new  ditch  crossing  jno- 
tion coordination _plan  code  was  simply  added.  If  the  original  motion  coordinator  logic  had  been  imbedded 
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in  the  "ASV"  robot  code  because  only  one  programming  paradigm,  such  as  an  imperative  paradigm,  had 
been  utilized  to  program  the  work  in  [15],  the  extension  to  a  ditch  crossing  capability  in  the  "ASV"  robot 
code  would  have  been  a  very  difficult  and  very  time  consuming  task. 

Overall,  the  development  and  coding  of  the  new  extended  "ASV"  and  motion  coordinator  clearly 
manifested  the  advantages  of  the  use  of  multiple  programming  paradigms  to  program  a  complex  robot 
motion  coordination  function  which  constandy  performs  on-line  optimization  like  a  human  or  an  animal 
coordinating  his  or  its  motion  based  on  sensory  information  and  learned  experiences.  Rule-based 
programming  to  express  logic,  object-oriented  programming  to  simulate  physical  and  functional  objects, 
and  a  numerical  processing  library  written  in  a  functional  or  imperative  language  to  implement  mathematics 
and  physics  needed  for  simulation  are  very  naturally  divided  components  to  simulate  a  complex  system, 
such  as  that  treated  in  this  report. 

One  of  major  complains  about  programs  using  Artificial  Intelligence  techniques  and  languages  is 
slow  execution  speed  in  on-line  computing  applications.  In  this  study  reported  here,  the  most  prominently 
visible  candidate  to  be  blamed  for  slow  execution  speed  is  Prolog  code.  However,  the  execution  speed  of 
Symbolics  Prolog  on  a  Symbolics  Lisp  machine  is  not  so  slow  as  might  be  expected.  It  is  only  slightly 
slower  than  that  of  Symbolics  Lisp  or  Flavors.  However,  the  execution  speed  of  Prolog  implementations 
on  other  machines  are  usually  considerably  slower  than  those  of  non-Prolog  implementations.  One 
solution  for  slow  Prolog  execution  speed  may  be  to  use  a  special  Prolog  processor,  such  a  Xenologic  X-l 
[27],  to  execute  Prolog  code.  The  other  solution  is  to  convert  the  Prolog  code  to  an  other  language,  such  as 
Lisp.  The  latter  approach  was  actually  adopted  to  test  the  correctness  of  the  program  developed  herein, 
using  a  TI  Explorer  machine  because  this  solution  is  readily  applicable  without  great  modification  to  the 
interface  between  Prolog  and  Lisp  codes,  and  because  a  TI  Explorer  machine  was  conveniently  available  to 
the  authors  in  an  office  environment.  Though  the  speed  gain  in  program  execution  is  little  over  that 
expected  with  Symbolics  Prolog,  this  approach  potentially  makes  a  much  wider  variety  of  computing 
hardware  suitable  to  execute  the  motion  coordination  program  developed  here.  Moreover,  this  approach  may 
provide  another  advantage  in  near  future  since  advances  in  microprocessors  based  on  RISC  or  CISC 
architecture  [28]  will,  with  respect  to  Lisp  execution  speed,  soon  equal  or  outperform  Lisp  machines  [20]. 

22 


Already,  with  respect  to  execution  speed  alone,  SPARC-based  Sun  workstations  narrow  the  large  gap 
previously  existing  between  a  Lisp  machine  and  a  conventional  machine  running  the  Lisp  language. 
Therefore,  rather  than  running  a  slow  Prolog  program  on  a  conventional  machine,  automatic  conversion 
from  Prolog  to  Lisp  after  a  development  phase  could  become  an  effective  way  to  achieve  markedly  better 
performance  if  the  program  were  to  be  tested  on  the  physical  ASV  walking  machine. 


7.  Summary  and  Recommendation 

The  main  purpose  of  this  study  was  to  demonstrate  the  value  of  a  multiple  pr*  •  ramming  paradigm 
approach  in  the  development  of  software  for  motion  coordination  for  the  ASV  walking  machine.  An 
important  secondary  goal  was  extending  the  work  in  [16]  so  that  the  ASV  can  cross  a  ditch  without  any 
assistance  from  a  human  operator.  Thus,  the  terrain  handling  capability  of  the  ASV  under  program  control 
was  extended  from  binary-type  terrain  to  ternary-type  terrain  for  the  first  time.  The  third  goal  was  to  take 
into  consideration  the  overlapping  working  volumes  of  the  legs  of  the  ASV  in  order  to  utilize  the  full 
kinematic  capability  that  the  vehicle  geometry  can  give.  This  later  factor  made  a  direct  contribution  in 
widening  the  maximum  ditch  width  (MDW)  that  the  vehicle  can  cross. 

The  approach  adopting  multiple  programming  paradigms  for  motion  coordination,  which  was 
proposed  in  [11]  and  implemented  in  [16],  again  exhibited  its  power.  First  of  all,  it  forced  a  well-organized 
and  functionally  clean  abstraction  hierarchy  for  a  complex  and  ill-defined  problem.  Secondly,  it 
considerably  reduced  development  time  and  effort  The  program  development  associated  with  this  report 
could  have  been  a  major  undertaking  if  a  single  programming  paradigm  had  been  utilized.  Instead,  as 
described  in  the  preceding  text,  with  the  approach  taken  here  most  of  the  program  in  [16]  is  reused,  while 
only  small  amount  of  code  is  additionally  written. 

The  code  translation  from  Prolog  to  Lisp  was  possible  because  the  Prolog  code  used  herein  was 
utilized  as  a  simple  rule-based  system.  This  success  of  this  translation  further  justifies  the  usage  of  Prolog 
as  one  of  the  languages  in  the  multiple  paradigm  environment  because  it  could  allow  much  wider  varieties 
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of  computing  hardware  to  execute  the  motion  coordination  program  developed.  Moreover,  this  actually 
made  the  program  execution  somewhat  faster  than  that  of  the  program  with  the  untranslated  Prolog  code. 

Among  studies  remaining  to  be  conducted  are  inclusion  of  vehicle  inertia  in  the  simulation,  effects 
of  leg  motion  on  the  location  of  the  vehicle  center  of  gravity,  and  a  better  simulation  of  the  vision  system. 
Such  a  study  would  be  appropriate  to  a  later  phase  of  this  research  along  with  an  investigation  of  further 
changes  to  the  Prolog  rule  set  to  enable  the  ASV  to  climb  over  large  obstacles  or  to  go  around  them  if  this 
is  not  possible. 
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Figure    2:    Hierarchy    of    simulation    objects 
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Cvclel  :State1 :  Place  Legs  in  the  Air 


il*^+JL+JL+^^^+i 


Cvclel  :State2:  Back  Middle  Leas 
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Cvclel  :State3:  Forward  Rear  Leas 
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Cvclel  :State4:  Forward  Middle  Leas 
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Cvclel  :State5:  Forward  Front  Leas 


Cvclel  :State6:  Lift  Middle  Leas  and  Move 


Figure   3:    Ditch    Crossing    Preparation    Phase 
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Cvcle2:State1 :  Back  Middle  Leas 


Cvcle2:State2:  Forward  Rear  Leas 


Cvcle2:State3:  Forward  Middle  Leas 


Figure  3:     Continued  .. 
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Figure  4:   Ditch  Crossing   Main   Phase 
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Figure  4:  Continued 
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;;-*-  Mode:PROLOGPackage:  robot-rulesBase:10  -*- 

robot :-  initialize,  repeat,  my_loop,  fail. 

initialize  :-  inits,  init_ditch_plan. 

init_dicth_plan  :-  retract(plan_cycle(_)).  retract(plan_state(_)X  fail. 
init_ditch_plan  :-  asserta(plan_cycle(l)), 

asserta(plan_state(place_legs_in_the_air)) . 

myjoop  :-  get_command,  plan,  execute, !. 

get_command  :-  X  is  readjoystick. 


plan  :-  ditch_mode,  ditch_crossing_motion_coordination_plan. 
plan :-  free_gaits_motion_coordination_plan. 

ditch_mode  :-  ditch_mode(in).    ;cleared  by  ditch_plan. 
ditch_mode :-  X  is  at_ditch_area,  X  =  t,  asserta(ditch_mode(in)). 


execute :-  execute_motion,  draw_robot, !. 
execute_motion  :-  X  is  execute_planned_motion. 
draw_robot :-  X  is  graphical_display. 


Figure  5:  Prolog  Program 
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;;    Free  Gaits  Motion  Coordination  Plan 


free_gaits_motion_coordination_plan :-  update_robot_state,  check_tkm_limit, 

leg_plan,  body_plan,  generate_decision, !. 


update_robot_state  :-  X  is  update_robot_status. 


check_tkm_limit :-  A_leg  is  at_tkm_limit,  A_leg  \=  nil, 

asserta(limit_leg(A_leg,lift)). 
check_tkm_limit. 

leg_plan :-  lift_a_leg. 
leg_plan :-  exchange_legs. 
leg_plan :-  stable. 
leg_plan :-  place_a_leg. 
leg_plan :-  wait_for_legs. 

stable  :-  Condition  is  stable_p,  Condition  =  L 

lift_a_leg  :-  stable,  A_leg  is  smallest_tkm_leg,  A_leg  \==  nil, 

Condition  is  stable_without(A_leg),  Condition  ==  t, 
asserta(decision(  A_leg,_,l  if t)) . 

exchange_legs  :-  stable,  LegA  is  smallest_tkm_leg,  LegA\=  nil, 
LegB  is  max_sm_leg(LegA),  LegB  \=  nil, 
Condition  is  has_more_tkm(LegB ,LegA), 
Condition  ==  t, 
asserta(decision(Leg  A  ,LegB  .exchange)). 

place_a_leg  :-  A_leg  is  max_sm_leg(_)»  A_leg\=  nil, 
asserta(decision(A_leg,_,place)). 

wait_for_legs :-  try_new_foothold. 
wait_for_legs :-  recovery,  asserta(reduce_speed). 
wait_for_legs :-  asserta(reduce_speed),  restore_limit_leg. 

try_new_foothold  :-  A_leg  is  leg_with_new_foothold,  A_leg  V==  nil, 
asserta(decision(A_leg,_,place)). 

recovery  :-  A_leg  is  do_recovery,  A_legV=  nil, 

asserta(decision(A_leg,_,place)),  restore_limit_leg. 

restore_limit_leg :-  retract(limit_leg(A_leg,lift)). 
restore_limit_leg. 


Figure  5:  Continued  ... 
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V 

;    Ditch  Crossing  Motion  Coordination  Plan 

.*******************************+************************** 


ditch_crossing_motion_coordination_plan  :-  ditch _plan_done,  retract(ditch_mode(in)),  idle_cycle. 
ditch_crossing_motion_coordination_plan :-  cycle_planner. 


********** 


Cycle  Planner  ************ 


ditch_plan_done :-  plan_cycle(6),  retract(plan_cycle(6)), 
asserta(plan_cycle(  1 )), 
prepare_next_ditch_plan. 

prepare_next_ditch_plan :-  move. 

cycle_planner :-  one_cycle_done,  plan_cycle(N),  Nl  is  N+l, 

retract(plan_cycle(N)),  asserta(plan_cycle(Nl)), 
idle_cycle. 

cycle_planner :-  plan_cycle. 


;;**********  Plan  Cycle  Dispatcher  ************ 


one_cycle_done :-  plan_state(one_plan_cycle_done), 

retract(plan_state(one_plan_cycle_done)), 
initialize_plan_state. 


initialize_plan_state :-  asserta(plan_state(start)). 

plan_cycle :-  plan_cycle(l),  update_robot_state,  ditch_plan_cycle_l, 

body_plan,  generate_decision,!. 
plan_cycle :-  plan_cycle(2),  update_robot_state,  ditch_plan_cycle_2, 

body_plan,  generate_decision,!. 
plan_cycle :-  plan_cycle(J),  update_robot_state,  ditch_plan_cycle_3, 

body_plan,  generate_decision,!. 
plan_cycle :-  plan_cycle(4),  update_robot_state,  ditch_plan_cycle_4, 

body_plan,  generate_decision,!. 
plan_cycle  :-  plan_cycle(5),  update_robot_state,  ditch_plan_cycle_5, 

body_plan,  generate_decision,!. 

idle_cycle :-  update_robot_state,  body_plan,  generate_decision, !. 

Figure  5:  Continued ... 
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**********     Cvr\pc    ********* 


Cycles 


ditch_plan_cycle_l  :-  plan_state(start),  retract(plan_state(start)), 
asserta(plan_state(place_legs_in_the_air)) , 
place_legs_in_the_air(back_middle_legs). 
place_legs_in_the_air(back_middle_legs). 
back_middleJegs(forward_rear_legs). 
fOTward_rear_legs(forward_middle_legs). 
forward_middle_legs(fOTward_fipont_legs). 

:-  forward_frontJegs(lift_middleJegs_and_move). 

:  -  lift_middle_legs_and_move(one_plan_cycle_done). 


ditch_plan_cycle_l 
ditch_plan_cycle_  1 
ditch_plan_cycle_  1 
ditch_plan_cycle_l 
ditch_plan_cycle_l 
ditch_plan_cycle_  1 


ditch _plan_cycle_2  :-  plan_state(start),  retract(plan_state(start)), 
asserta(plan_state(back_m  iddlejegs)) , 
back_middle_legs(forward_rear_legs). 

ditch_plan_cycle_2 :-  back_middle_legs(forward_rear_legs). 

ditch_plan_cycle_2 :-  forward_rear_legs(forward_middle_legs). 

ditch_plan_cycle_2 :-  forward_middle_legs(one_plan_cycle_done). 


ditch_plan_cycle_3 :-  plan_state(start),  retract(plan_state(start)), 

asserta(plan_state(move_forward_front_legs)), 
move_forward_front_legs(move_forward_tniddle_legs). 

ditch_plan_cycle_3  :-  move_forward_front_legs(move_back_middle_legs). 

ditch_plan_cycle_3 :-  move_back_middle_legs(move_forward_rear_legs). 

ditch_plan_cycle_3 :-  move_forward_rear_legs(one_plan_cycle_done). 


ditch_plan_cycle_4 :-  plan_state(start),  retract(plan_state(start)), 

asserta(plan_state(move_forward_middle_legs)), 
move_forward_middle_legs(one_plan_cycle_done). 

ditch_plan_cycle_4  :-  move_forward_middle_legs(onej)lan_cycle_done). 


ditch_plan_cycle_5  :-  plan_state(start),  retract(plan_state(start)), 

asserta(plan_state(move_forward_front_legs)), 
move_forward_frontJegs(move_forward_middle_legs). 

ditch_plan_cycle_5 :-  move_forward_front_legs(move_back_middle_legs). 

ditch_plan_cycle_5 :-  move_back_middle_legs(move_forward_rear_legs). 

ditch_plan_cycle_5 :-  move_forward_rear_legs(one_plan_cycle_done). 


Figure  5:  Continued 
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.**************  states  *************** 


;;;;;  back_middle_legs  subgroup 

back_middle_legs(Next_State) :-  plan_state(back_middle_legs), 

back_middle_legs_done, 
retract(plan_state(back_middle_legs)) , 
asserta(plan_state(Next_State)), 
stop. 
back_middle_legs(Next_State) :-  plan_state(back_middle_legs), 

do_back_middle_legs, 
stop. 


;;;;;  forward_front_legs  subgroup 

forward_front_legs(Next_State) :-  plan_state(forward_front_legs)), 

forward_front_legs_done, 
retract(plan_state(forward_&ont_legs) , 
asserta(plan_state(Next_State)), 
stop. 

forward_front_legs(Next_State) :-  plan_state(forward_front_legs)), 

do_forward_front_legs, 
stop. 


;;;;;  forward_middle_legs  subgroup 

forward_middle_legs(Next_State) :-  plan_state(forward_middle_legs), 

forward_middle_legs_done, 
retract(plan_state(forward_middle_legs), 
asserta(plan_state(Nex  t_S  tate)) , 
stop. 

forward_middle_legs(Next_State) :-  plan_state(forward_middle_legs)) 

do_forward_middle_legs, 
stop. 


;;;;;  forward_rear_legs  subgroup 

forward_rear_legs(Next_State) :-  plan_state(forward_rear_legs), 

forward_rear_legs_done, 
retract(plan_state(forward_rear_legs), 
asserta(plan_state(Next_State)) , 
stop. 

forward_rear_legs(Next_State) :-  plan_stite(forward_rear_legs), 

do_forward_rear_legs, 
stop. 


;;;;;  lift_middle_legs_and_move  subgroup 

lift_rniddle_legs_and_rnove(Next_State):-plan_state(lift_middle_legs_and_move), 

move_done,  stop, 


Figure  5:  Continued ... 
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retract(plan_state(lift_middle_legs_and_move)), 
asserta(plan_state(Next_State)). 
lift_middle_legs_and_move(Next_State)  :-plan_state(lift_middle_legs_and_move), 

do_lift_middle_legs,  move. 


;;;;;  move_back_middle_legs  subgroup 

move_back_middle_legs(Next_State) :-  plan_state(move_back_middle_legs), 

move_back_middle_legs_done, 
retract(plan_state(move_back_middle_legs)), 
asserta(plan_state(Next_State)). 

move_back_middle_legs(Next_State) :-  plan_state(move_back_middle_legs), 

do_move_back_middle_legs. 


;;;;;  move_forward_front_legs  subgroup 

move_forward_front_legs(Next_State) :-  plan_state(move_forward_front_legs), 

move_forward_firont_legs_done, 
retract(plan_state(move_forward_front_legs)), 
asserta(plan_state(Nex  t_S  tate)) . 

move_forward_front_legs(NexuState) :-  plan_state(move_forward_front_legs), 

do_move_forward_firont_legs. 


;;;;;  move_forward_middle_legs  subgroup 

move_forward_middle_legs(Next_State)  :-pIan_state(move_forward_middle_legs), 

move_forward_middleJegs_done, 
retract(plan_state(move_forward_front_legs)), 
asserta(plan_state(Next_S  tate)) . 

move_forward_middle_legs(Next_State) :-  plan_state(move_forward_middle_legs), 

do_move_forward_middle_legs. 


;;;;;  move_forward_rear_legs  subgroup 

move_forward_rear_legs(Next_State) :-  plan_state(move_forward_rear_legs), 

move_forward_middle_legs_done, 
retract(plan_state(move_forward_rearJegs))) 
assena(plan_state(Nex  t_S  tate)) . 

move_forward_rear_legs(Next_State) :-  plan_state(move_forward_rear_legs), 

do_move_forward_rear_legs . 


;;;;;  place_legs_in_the_air  subgroup 

place_legs_in_the_air(Next_State) :-  plan_state(place_legs_in_the_air), 

place_legs_in_the_air_done, 
retract(plan_state(place_legs_in_the_air)), 
asserta(plan_state(Next_state)), 
stop. 

place_legs_in_the_air(Next_State) :-  plan_state(place_legs_in_the_air), 

do_place_legs_in_the_air,  stop. 


Figure  5:  Continued 
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..****************  State  Executors  ******************** 


;;;;;  do_back_middle_legs  subgroup 

back_middle_legs_done :-  all_middle_legs_lifted,  all_middle_legs_placed, 

clear_middle_lifted_memory,clear_move_memory. 
do_back_middle_legs :-  all_middle_legs_lifted,  place_middle_legs_back. 
do_back_middle_legs :-  lift_middle_legs. 

all_middle_legs_ufted :-  middle_legs(lifted). 
all_middle_legs_lified  :-  X  is  both_middle_legs_lifted,  X  =  t, 
asserta(middle_legs(lif ted)) . 

all_middle_legs_placed :-  X  is  both_middleJegs_placed,  X  =  t. 

clear_middle_lifted_memory :-  retract(middle_legsOifted)). 

place_middle_legs_back  :-  A_leg  is  placable_middle_leg,  A_legV=  nil, 

asserta(decision(A_leg,_,place_back)). 
place_middle_legs_back. 

lift_middle_legs  :-  A_leg  is  liftable_middle_leg,  A_leg  V=  nil, 

asserta(decision(A_leg,_,lift)). 
uft_middle_legs. 


;;»;  do_forward_front_legs  subgroup 

forward_front_legs_done :-  all_front_legs_lifted,  all_front_legs_placed, 

clear_front_lifted_memory ,  clear_move_memory . 

do_forward_front_legs :-  all_front_legs_lifted,  place_front_legs. 
do_forward_front_legs :-  lift_front_legs. 

all_front_legs_lifted :-  front_legs(lifted). 
all_front_legs_lifted  :-  X  is  both_front_legs_lifted,  X  =  t, 
asserta(front_legs(lifted)). 

all_front_legs_placed  :-  X  is  both_front_legs_placed,  X  =  L 

clear_front_lifted_memory :-  retract(front_legs(lifted)). 

place_front_legs  :-  A_leg  is  placable_front_leg,  A_leg  \==  nil, 

asserta(decision(A_leg,_,place)). 
place_front_legs. 

lift_front_legs  :-  A_leg  is  liftable_front_leg,  A_leg  W  nil, 

asserta(decision(A_leg,_,lirt)). 
lift_front_legs. 


Figure  5:  Continued 
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;;;;;  do_forward_middle_legs  subgroup 

forward_middle_legs_done :-  all_middle_legs_lifted,  all_middle_legs_placed, 

clear_middle_lifted_memory,  clear_move_memory. 

do_forward_middle_legs :-  all_middle_legs_lifted,  place_middle_legs. 
do_forward_middle_legs :-  lift_middle_legs. 

place_middle_legs  :-  A_leg  is  placable_middle_leg,  A_leg  W  nil, 

asserta(decision(A_leg,_,place)). 
place_middle_legs. 


;;;;*,  do_forward_rear_legs  subgroup 

forward_rear_legs_done :-  aU_rear_legs_lifted,  all_rear_legs_placed, 

clear_rear_lifted_memory,  clear_move_memory. 

do_forward_rear_legs :-  all_front_legs_lifted,  place_rear_legs. 
do_forward_rear_legs :-  liftjrearjegs. 

all_rear_legs_lifted :-  rearjegs(lifted). 
all_rear_legs_lifted  :-  X  is  both_rear_legs_lifted,  X  =  t, 
asserta(rear_legs(lifted)). 

all_rear_legs_placed :-  X  is  both_rear_legs_placed,  X  =  t. 

clear_rear_lifted_memory :-  retract(rear_legs(lifted)). 

place_rear_legs :-  A_leg  is  placable_rear_leg,  AJeg  V=  nil, 

asserta(decision(A_leg  ,_,place)). 
place_rear_legs. 

lift_rear_legs  :-  A_leg  is  uftable_rear_leg,  A_leg\==  nil, 

asserta(decision(A_leg,_,lift)). 
lift_rear_legs. 


;;;;;  do_lift_middle_legs  subgroup 
do_lift_middle_legs :-  lift_middle_legs. 

;;;;;  do_move_back_middle_legs  subgroup 

move_back_middle_legs_done :-  all_middle_legs_lifted,  all_middle_legs_placed, 

clear_middle_lifted_memory,  clear_move_memory, 
stop. 
do_move_back_middle_legs  :-  all_middle_legs_lifted,  move_done,  stop, 

place_middle_legs_back. 
do_move_back_middle_legs :-  all_middle_legs_lifted,  move. 
do_move_back_middle_legs :-  lift_middle_legs,  stop. 

Figure  5:  Continued  ... 
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;;;;;  do_move_forward_front_legs  subgroup 

move_forward_front_legs_done :-  all_front_legs_lifted,  all_front_legs_placed, 

clear_front_lifted_memory,  clear_move_memory, 
stop. 
do_move_forward_front_legs :-  all_front_legs_lifted,  move_done,  stop, 

place_front_legs. 
do_move_forward_front_legs :-  all_front_legs_lifted,  move. 
do_move_forward_front_legs :-  lift_front_legs,  stop. 


»;;;  do_move_forward_middle_legs  subgroup 

move_forward_middle_legs_done :-  all_middle_legs_lifted, 

all_middle_legs_placed, 
clear_middle_lifted_memory, 
clear_move_memory,  stop. 

do_move_forward_middle_legs  :-  all_middle_legs_lifted,  move_done,  stop, 

place_middle_legs. 

do_move_forward_middle_legs :-  all_middle_legs_lifted,  move. 

do_move_forward_middle_legs :-  lift_middle_legs,  stop. 


;;;;;  do_move_fonvard_rear_legs  subgroup 

move_forward_rear_legs_done :-  all_rear_legs_lifted,  all_rear_legs_placed, 

clear_rear_lifted_memory,  clear_move_memory, 
stop. 
do_move_forward_rear_legs :-  all_rear_legs_lifted,  move_done,  stop, 

place_rear_legs. 
do_move_forward_rear_legs :-  all_rearjegs_lifted,  move. 
do_move_forward_rear_legs :-  lift_rear_legs,  stop. 


;;;;;  place_legs_in_the_air  subgroup 

place_legs_in_the_air_done  :-  X  is  all_legs_placed,  X  =  L 

place_legs_in_the_air :-  A_leg  is  placable_leg,  A_leg  \=  nil, 

asserta(decision(A_leg,_,place)). 
place_legs_in_the_air. 

;;;;;  body_movement  subgroup 

move :-  asserta(resume_movement). 
stop :-  asserta(stop_movement). 

clear_move_memory :-  retract(move(done)). 
clear_move_memory. 

move_done :-  move(done). 

move_done  :-  X  is  at_tkm_limit,  X  V=  nil,  asserta(move(done)). 

move_done  :-  X  is  at_stability_limit,  X  \=  nil,  asserta(move(done)). 

Figure  5:  Continued  ... 
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+********************************************************* 


Plan  Libraries 

;;;;;  body_plan  subgroup 

body_plan  :-  speed_plan,  trajectory _plan. 

speed_plan :-  retract(reduce_speed),  slow_down. 
speed_plan :-  speed_up. 

speed_up  :-  X  is  speed_up_robot. 

slow_down  :-  X  is  slow_down_robot. 

trajectory_plan :-  stable_m,  restore_trajectory. 
trajectory  _plan :-  modify_trajectory. 

stable_m  :-  Condition  is  stable_p_m,  Condition  ==  t. 

restorejrajectory :-  X  is  restore_command. 

modify _trajectory  :-  X  is  modify _command. 

;;;;;  generate_decision  subgroup 

generate_decision :-  retract(decision(A_leg,B_leg,A_decision)), 

X  is  send_decision(A_leg,BJeg,A_decision),  fail. 

generate_decision :-  retract(limit_leg(A_leg,A_decision))) 

X  is  send_decision(A_leg,_vA_decision),  fail. 

generate_decision . 


Figure  5:  Continued 
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Appendix 


Lisp  Code  for  ASV  Simulation 


body-controller-t .li3p         Wed  Nov  28  10:11:54  1990         1 

;;;  -*-  Mode : Common-Lisp;  Base: 10  -*- 
.***************************************************************** 

body-controller  definition 
***************************************************************** 

(def flavor  body-controller (joystick-command-regulator  terrain-regulator 

H-calculator 

body-trans-ratel  body-rotate-rat el 
body-trans-rate6  body-rotate-rate 6 
body-trans-rate 10  body-rotate-rate 10 
HI  inv-Hl  H6  inv-H6  H10  inv-HIO 
H  inv-H  body-t  body-r) 

0 

: initable-instance-variables) 


(defmethod  (body-controller  :initti) 
0 
(setf  joystick-command-regulator  (make-instance  ' joystick-command-regulator) ) 
(setf  terrain-regulator  (make-instance  'terrain-regulator)) 
(setf  H-calculator  (make-instance  'H-calculator)) 
(send  joystick-command- regulator  :initti) 
(send  terrain-regulator  :initti) 
(setf  H  (send  H-calculator  :initti)) 
(send  self  : init-body-rates) 
(send  self  :init-H) 
HI 
) 


(defmethod  (body-controller  : init-body-rates) 
0 
(setf  body-trans-ratel  '  (0  0  0) ) 
(setf  body-trans-rate6  '(0  0  0)) 
(setf  body-trans-ratelO  '(0  0  0)) 
(setf  body-rotate-ratel  ' (0  0  0) ) 
(setf  body-rotate-rate6  '(0  0  0)) 
(setf  body-rotate-ratelO  '(0  0  0)) 
) 


(defmethod  (body-controller  :init-H) 

0 
;  library  fucntion  :  ident 

(setf  HI   H) 

(setf  H6   H) 

(setf  H10  H)  , 

(setf  inv-H  (matrixinv  H) ) 

(setf   invHl    inv-H) 

(setf    inv-H6    inv-H) 

(setf    inv-HIO    inv-H) ) 


body-controller-t .lisp         Wed  Nov  28  10:11:54  1990 


(defmethod  (body-controller  :control) 

(joystick-command  deceleration-factor  estimated-support-plane) 
(setf  H  HI) 

(send  self  :update  joystick-command  deceleration-factor  estimated-support-plane) 
(send  self  :save) 
(dotimes  (i  10) 

(cond  ( (equal  i  0) 

(setf  body-trans-ratel  body-t) 
(setf  body-rotate-ratel  body-r) 
(setf  HI  H) 
(setf  inv-Hl  inv-H) ) 
( (equal  i  5) 
(setf  body-trans-rate6  body-t) 
(setf  body-rotate-rate6  body-r) 
(setf  H6  H) 
(setf  inv-H6  inv-H) ) 
(  (equal  i  9) 
(setf  body-trans-ratelO  body-t) 
(setf  body-rotate-ratelO  body-r) 
(setf  H10  H) 
(setf  inv-HIO  inv-H))) 
(send  self  : update  joystick-command  deceleration-factor  estimated-support-plane) 
) 
(send  self  : restore)) 


(defmethod  (body-controller  : update) 

(joystick-command  deceleration-factor  estimated-support-plane) 
;  internally  used  by  control  method 

(let*  ( (t-command  (send  terrain-regulator  : regulate 

estimated-support-plane  H) ) 
(j-command  (send  joystick-command-regulator  : regulate 

joystick-command  deceleration-factor) ) 
) 
(setf  body-t  (list  (first  j-command)  (second  j-command) 

(third  t-command) ) ) 
(setf  body-r  (list  (first  t-command)  (second  t-command) 

(third  j-command) )  ) 
(setf  H  (send  H-calculator  :new-H  body-t  body-r) ) 
(setf  inv-H  (matrixinv  H)  )  ) ) 


(defmethod  (body-controller  : restore) 
0 

;  internally  used  by  control  method 

(send  joystick-command-regulator  : restore) 
(send  terrain-regulator  : restore) 
(send  H-calculator  .-restore)) 


(defmethod  (body-controller  :save) 
(-) 

;  internally  used  by  control  method 

(send  joystick-command-regulator  :save) 
(send  terrain-regulator  :save) 
(send  H-calculator  :save)) 


body-controller-t .lisp         Wed  Nov  28  10:11:54  1990 


(defmethod  (body-controller  :get-body-trans-ratel) 
0 
body-trans -rate 1) 


(defmethod  (body-controller  :get-body-rotate-ratel) 
0 
body-rot ate-ratel) 


(defmethod  (body-controller  :get-body-trans-ratelO) 
0 
body-trans-ratelO) 


(defmethod  (body-controller  :get-body-rotate-ratelO) 
0 
body-rotate-ratelO) 


(defmethod  (body-controller  :get-Hl) 
() 
HI) 


(defmethod  (body-controller  :get-inv-Hl) 
0 
inv-Hl) 


(defmethod  (body-controller  :get-H6) 
0 
H6) 


(defmethod  (body-controller  :get-inv-H6) 
0 
inv-H6) 


(defmethod  (body-controller  :get-H10) 
0 
H10) 


(defmethod  (body-controller  :get-inv-H10) 
0 
inv-HIO) 


body-t.lisp         Wed  Nov  28  10:11:59  1990 

;;;  -*-  Mode : Common-Lisp ;  Base: 10  -*- 


********************************************************* 


body  flavor  definition 


********************************************************* 


(def flavor  body (stability-calculator  support-plane-estimator 

body-controller  owner 
e st imated-support -plane 
deceleration-factor 
support -plane-clock 
modify- vector 
modif y-vector-p 
st op-mot ion- flag 
joy-command) 
0 
:  init able- instance- variables) 


(defmethod  (body  : slow-down) 
() 
(setf  deceleration-factor  (+  deceleration-factor  1)  ) 
(if  (>  deceleration-factor  20) 

(setf  deceleration-factor  20) ) ) 

(defmethod  (body  : speed-up) 
0 
(setf  deceleration-factor  (-  deceleration-factor  1)  ) 
(if  (<  deceleration-factor  0) 

(setf  deceleration-factor  0))) 


(defmethod  (body  :stable-m) 
(supporting- legs) 
(send  stability-calculator  :stable-m 

supporting-legs  (send  body-controller  :get-H10)  )  ) 


(defmethod  (body  :stable-p-m) 

(supporting-p-legs  a-leg) 
(send  stability-calculator  :stable-p-m 

supporting-p-legs 
(send  body-controller  :get-Hl) ) ) 


(defmethod  (body  :stop-p) 
0 
(let  ((trans-rate  (send  self  :get-body-trans-ratel) ) ) 
(equal  (list  (first  trans-rate) 

(second  trans-rate) ) 
'  (0.0  0.0)))) 


(defmethod  (body  : modif y-command) 
0 
(setf  modify-vector 
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(send  stability-calculator  : get-recovery-vector) ) ) 


(defmethod  (body  :modify-command-p) 

0 

(setf  modify-vector-p 

(send  stability-calculator  :get-recovery-vector-p) ) ) 


(defmethod  (body  : restore-command) 
0 
(setf  modify-vector  '(0  0  0))) 


(defmethod  (body  : restore-command-p) 
0 
(setf  modify-vector-p  '(0  0  0))) 


(defmethod  (body  : stop-motion) 
(a-leg) 
(setf  stop-motion-flag  a-leg) 


(defmethod  (body  : restore-motion) 
0 
(setf  stop-motion-flag  nil) ) 


(defmethod  (body  :initti) 
0 
(setf  deceleration-factor  0) 
(setf  modify-vector-p  ' (0  0  0)) 
(setf  modify-vector  '(0  0  0)) 
(setf  stop-motion-flag  nil) 
(setf  support -plane-clock  10) 
(setf  stability-calculator 

(make-instance  'stability-calculator) ) 
(setf  support -plane-estimator 

(make-instance  'support-plane-estimator) ) 
(setf  body-controller 

(make-instance  'body-controller) ) 
(send  stability-calculator  :initti) 
(send  support-plane-estimator  :initti) 
(send  body-controller  :initti) 
) 

(defmethod  (body  : get -modify-vector) 
0 
(vectsub  modify-vector 

(dotprod  modify-vector 

(normalize -vector  joy-command) ) ) ) 


(defmethod  (body  :get-modif y-vector-p) 
0 
modify-vector-p) 
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(defmethod  (body  : calculate-motion) 
(joystick-command  legs) 
(setf  joy-command  joystick-command) 
(cond  ( (equal  support-plane-clock  10) 
;  ???  bug  ??? 

(setf  estimated-support-plane 

(send  support-plane-estimator  :get-plane  legs)) 
(setf  support-plane-clock  0))) 
(setf  support-plane-clock  (+  support-plane-clock  1) ) 
(cond 

( (or  stop-motion-flag  (null  modify-vector-p) ) 
(send  body-controller  :control 
'(0  0  0) 

0  estimated-support-plane) ) 
(modify-vector-p 
(send  body-controller  : control 

(vectadd  joy-command  (send  self  :get-modif y-vector-p) ) 
deceleration-factor  estimated-support-plane)  ) 
(t 
(control  body-controller 

(vectadd  joy-command  (3end  self  :get-modif y-vector) ) 
deceleration-factor  estimated-support-plane)  )  )  ) 


(defmethod  (body  :get-estimated-support-plane) 
0 
estimated-support-plane) 


(defmethod  (body  :get-body-trans-ratel) 
0 
(send  body-controller  :get-body-trans-ratel) ) 


(defmethod  (body  :get-body-rotate-ratel) 
0 
(send  body-controller  :get-body-rotate-ratel) ) 


(defmethod  (body  :get-body-trans-ratelO) 
0 
(send  body-controller  :get-body-trans-ratelO) ) 


(defmethod  (body  :get-body-rotate-ratelO) 
0 
(send  body-controller  :get-body-rotate-ratelO) ) 


(defmethod  (body  :get-Hl) 
0 
(send  body-controller  :get-Hl) ) 


(defmethod  (body  :get-inv-Hl) 
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0 
(send  body-controller  :get-inv-Hl) ) 


(defmethod  (body  :get-H6) 

0 

(send  body-controller  :get-H6) ) 


(defmethod  (body  :get-inv-H6) 

0 

(send  body-controller  :get-inv-H6) ) 


(defmethod  (body  :get-H10) 
0 
(send  body-controller  :get-H10) ) 


(defmethod  (body  :get-inv-H10) 
0 
(send  body-controller  :get-inv-H10) 


(defmethod  (body  :more-stable) 

(supporting-legs  legl  leg2) 
(send  stability-calculator  :more-stable 

supporting-legs  (send  body-controller  :get-H10) 
legl  leg2) ) 


(defmethod  (body  : stable) 

(supporting-legs) 
(send  stability-calculator  : stable 

supporting-legs  (send  body-controller  :get-H10))) 


(defmethod  (body  :stable-p) 

( support ing-p-legs) 
(send  stability-calculator  :stable-p 

supporting-p-legs  (send  body-controller  :get-Hl) ) ) 
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...  _*_  Mode : Common-Lisp;  Base: 10  -*- 

********************************************************** 

regulator  flavor  definition 
********************************************************** 


(def flavor  regulator ( (max-a  3.2174)  (time-const  0.5)  (sampling-time  0.1)) 
0 
:  init able -instance -variables) 


(defmethod  (regulator  : filter) 

(desired-x  present-x) 
;  first  order  regulation 

(let  ( (del-vel  (/  (-  desired-x  present-x)  time-const))) 
(+  (*  (send  self  :g-limitor  del-vel)  sampling-time) 
present-x) ) ) 


(defmethod  (regulator  :g-limitor) 

(del-vel) 
;  limit  acceleration  to  3.2174  ft/ (sec*sec)   or  0.1  G. 
(cond  ( (>  del-vel  max-a)    max-a) 

( (<  del-vel  (-  max-a) )  (-  max-a) ) 
(T  del-vel) ) ) 


********************************************************** 

joystick-command-regulator  flavor  definition 

********************************************************** 


(def flavor  joystick-command-regulator (body-trans-rate-x 

body-trans -rate -y 
body-rotate-rate-z 
old-body-trans -rate-x 
old-body-trans -rate-y 
old-body-rotate-rate-z) 
(regulator) 
: initable-instance-variables) 


(defmethod  (joystick-command-regulator  rinitti) 
0 
(setf  body--trans-rate-x  0.0) 
(setf  body-trans-rate-y  0.0) 
(setf  body-rotate-rate-z  0.0) 
(list  body-trans-rate-x  body-trans-rate-y  body-rotate-rate-z) ) 
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(defmethod  (joystick-command-regulator  :  regulate) 
(joystick-command  decele rat ion- factor) 
(if  (<=  deceleration-factor  0) 

(setf  deceleration-factor  0.5))   ;  remove  effect  of  deceleration-factor, 
(let*  ( (d-const  0 .5) 

(x  (*  (first   joystick-command)  (/  d-const  deceleration-factor))) 
(y  (*  (second  joystick-command)  (/  d-const  deceleration-factor) ) ) 
(r  (*  (third   joystick-command)  (/  d-const  deceleration-factor)))) 
(setf  body-trans-rate-x   (send  self  : filter  x  body-trans-rate-x) ) 
(setf  body-trans-rate-y   (send  self  :  filter  y  body-trans-rate-y) ) 
(setf  body-rotate-rate-z  (send  self  :filter  r  body-rotate-rate-z) ) ) 
(if  (<  (abs  body-trans-rate-x)   0.02)   (setf  body-trans-rate-x   0.0)) 
(if  (<  (abs  body-trans-rate-y)   0.02)   (setf  body-trans-rate-y   0.0)) 
(if  (<  (abs  body-rotate-rate-z)  0.005)   (setf  body-rotate-rate-z   0.0)) 
(list  body-trans-rate-x  body-trans-rate-y  body-rotate-rate-z) ) 


(defmethod  (joystick-command-regulator  :  restore) 

0 

(setf  body-trans-rate-x  old-body-trans-rate-x) 

(setf  body-trans-rate-y  old-body-trans-rate-y) 

(setf  body-rotate-rate-z  old-body-rotate-rate-z) 

(list  body-trans-rate-x  body-trans-rate-y  body-rotate-rate-z) 


(defmethod  ( joystick-command-regulator  :save) 
0 
(setf  old-body-trans-rate-x  body-trans-rate-x) 
(setf  old-body-trans-rate-y  body-trans-rate-y) 
(setf  old-body-rotate-rate-z  body-rotate-rate-z) 
(list  body-trans-rate-x  body-trans-rate-y  body-rotate-rate-z) ) 
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;;;  -*-  Mode : Common-Lisp;  Base: 10  -*- 

************************************************************** 

state  flavor  definition 
************************************************************** 

(def flavor  state (name  next-state) 
0 
: initable-instance-variables) 


(defmethod  (state  : state-name) 
0 
name) 


(defmethod  (state  : set-next-state) 
(a-state) 
(setf  next-state  a-state)  ) 


************************************************************ 


sync-state  flavor  definition 


************************************************************ 


(def flavor  sync-state ( (time  0)  (del-t  0.1)  time-out) 
(state) 
: initable-instance-variables) 


(defmethod  (sync-state  :change) 
0 
(setf  time  (+  time  del-t) ) 
(cond  ( (>=  time  time-out) 
(setf  time  0) 
next-state) 
(t  self) ) ) 


(defmethod  (sync-state  :get-time) 
0 
time) 


********************************************************** 
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async-state  flavor  definition 
********************************************************** 


(def flavor  async-state ( (command  nil)  (observation  nil)) 
(state) 
:init able -instance -variables) 


(defmethod  (async-state  : change) 

(given-command  observed-event) 
(cond  ( (and  (not  observation) 

(equal  given-command  command) ) 
next-state) 
(  (and  (not  command) 

(equal  observed-event  observation)  ) 
next-state) 
(t  self))) 


******************************************************** 


state-machine  flavor  definition 


******************************************************** 


(def flavor  state-machine (state  owner) 
0 
:  initable-instance-variables) 


(defmethod  (state-machine  : state-name) 
0 
(send  state  : state-name) ) 


******************************************************* 

control-state-machine  flavor  definition 
******************************************************* 

(def  flavor  control-state-ntachine  (  (command  nil)  (observation  nil) 

contact-sensor  executor) 
(state-machine) 
: initable-instance-variables) 
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(defmethod  (control-state-machine  :initti) 
(leg-name) 
(if  (member  leg-name  ' (legl  leg4  leg5) ) 

(send  self  : init-control-machine  'support) 
(send  self  : init-control-machine  'ready)) 
(setf  contact-sensor  (send  owner  -.contact-sensor)) 
(setf  executor  (send  owner  :executor) ) ) 


(defmethod  (control-state-machine  : init-control-machine) 

(a-state-name) 
;  internally  used  by  init  method 

(let  (return  lift  support  contact  descent  advance  ready) 
(setf  return 

(make-instance  'sync-state 

:name  'return  : time-out  0.6)) 
(setf  lift 

(make-instance  ' sync-state 

:name  'lift  : time-out  0.4 
:next-state  return) ) 
(setf  support 

(make-instance  'async-state 

:name  'support  :command  ' recover-command 
:next-state  lift)) 
(setf  contact 

(make-instance  'sync-state 

:name  'contact  : time-out  1.0 
:next-state  support)) 
(setf  descent 

(make-instance  'async-state 

:name  'descent  :observation  'contact-confirm 
:next-state  contact) ) 
(setf  advance 

(make-instance  'sync-state 

:name  'advance  : time-out  0.6 
:next-state  descent) ) 
(setf  ready 

(make-instance  'async-state 

:name  'ready  : command  'deploy-command 
:next-state  advance) ) 

(send  return  : set-next-state  ready) 


(setf  state  (cond  ((equal  a-state-name  (send  ready  : state-name) ) 

ready) 
((equal  a-state-name  (send  advance  : state-name) ) 

advance) 
((equal  a-state-name  (send  descent  :state-name) ) 

descent) 
((equal  a-state-name  (send  contact  :state-name) ) 

contact) 
((equal  a-state-name  (send  support  :state-name) ) 

support) 
((equal  a-state-name  (send  lift  : state-name) ) 

lift) 
((equal  a-state-name  (send  return  : state-name) ) 

return) ) ) 
) 
) 
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(defmethod  (control-state-machine  :change  :before) 
0 
(cond  ( (typep  state  ' async-state) 

(if  (contact-sensor  :sensing) 

(setf  observation  'contact-confirm) 
(setf  observation  nil) ) 
))) 


(defmethod  (control-state-machine  : change) 
0 
(cond  ((typep  state  'async-state) 

(if  (send  contact-sensor  :sensing) 

(setf  observation  'contact-confirm) 
(setf  observation  nil) ) 
)) 

(cond  ((typep  state  'sync-state) 

(setf  state  (send  state  :change))) 
(t  (setf  state  (send  state  :change  command  observation)))) 
;  ) 


/(defmethod  (control-state-machine  :change  :after) 
0 
;  send  command  to  executor  with  sync-state-time 
(send  executor  :send-command  (send  state  : state-name) ) 
(if  (typep  state  'sync-state) 

(send  executor  :set-time  (send  state  :get-time) ) 
(send  executor  :set-time  nil))) 


(defmethod  (control-state-machine  : send-command) 
(a-command) 
(setf  command  a-command)  ) 
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;;;  -*-  Mode : Common-Lisp;  Base:  10  -*- 

.********************************************************** 

display .global s 
.********************************************************** 

(defvar  eye-space  nil) 
(defvar  middle-of-screen  nil) 


(defvar  terrain- joystick) 
(defvar  graph-terrain) 
(defvar  graph-asv) 


********************************************************** 


display .library 


********************************************************** 


(defun  draw-to-earth  (a-point) 

(let  ( (draw-pt  (make-displayable 
middle-of-screen 

(transform  eye-space  a-point)))) 
(draw-to 

(list  (truncate  (first  draw-pt) ) 

(truncate  (second  draw-pt) ) ) 
*robot-window*) ) ) 


(defun  draw-to-earth-d  (a-point) 
(let  ( (draw-pt  (make-displayable 
middle-of-screen 

(transform  eye-space  a-point) ) ) ) 
(draw-to-d 

(list  (truncate  (first  draw-pt)) 

(truncate  (second  draw-pt) ) ) 
*robot-window*)  )  ) 


(defun  erase-to-e  irth  (a-point) 
(let  ( (draw-pt  (make-displayable 
middle-of-screen 
(transform  eye-space  a-point) ) ) ) 
(erase-to 

(list  (truncate  (first  draw-pt) ) 

(truncate  (second  draw-pt) ) ) 
*robot-window*) )  ) 


(defun  eye-trans  (eye-pt) 
;  eye-pt  (radius  alpha  beta) 
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;  eye :=  orient* trans (0, 0, -r) *rot  (x, -beta) *rot (y, -alpha) *trans (-x,  -y,  -z) 
;  returns  eye-space 

;  library  :  ident,  transmat, rotate, matrixmult 
(let*  ((orient  (ident)) 

(rot  nil)  (trans  nil)  (eye  nil) 

(radius  (first  eye-pt) )  (alpha  (second  eye-pt) )  (beta  (third  eye-pt) ) 

(center-of-interest  (list  (/  (send  graph-terrain  :max-x)  2) 

(/  (send  graph-terrain  :max-y)  2)  0))) 
(setf  (aref  orient  2  2)  -1.0) 
(setf  trans  (transmat  0  0  (-  radius))) 
(setf  eye  (matrixmult  orient  trans) ) 
(setf  rot  (rotatemat  'y-axis  (-  alpha))) 
(setf  eye  (matrixmult  eye  rot)) 
(setf  rot  (rotatemat  'x-axis  (-  beta))) 
(setf  eye  (matrixmult  eye  rot) ) 

(setf  trans  (transmat  (-  (first  center-of-interest) ) 
(-  (second  center-of-interest) ) 
(-  (third  center-of-interest) ) ) ) 
(matrixmult  eye  trans))) 


(defun  make-displayable   (middle  pt) 
(let  ((scale    5000.0) 

(x  (first  pt))  (y  (second  pt) )  (z  (third  pt) ) ) 
(list  (+  (*  scale  (/  x  z) )  (first  middle)) 
(+  (*  scale  (/  y  z) )  (second  middle))))) 


(defun  move-to-earth  (a-point) 

(let  ( (draw-pt  (make-displayable 
middle -of -screen 

(transform  eye-space  a-point) ) ) ) 
(move-to 

(list  (truncate  (first  draw-pt) ) 
(truncate  (second  draw-pt)))))) 


************************************************** 


joystick  flavor  definition 


************************************************** 


(def flavor  joystick (( joy-x  0)  (joy-y  0)  (joy-r  0) ) 
0 
: initable-instance-variables) 


(defmethod  (joystick  :get- joy-value) 
0 
(let*  (  (key-value) 

(delta-x  0.2)  (delta-y  0.1)  (delta-r  0.01)) 
(setf  key-value  (my-read-char-no-hang) ) 

(cond  ((equal  key-value  '#\f)  (setf  joy-x  (+  joy-x  delta-x))) 
((equal  key-value  ' #\b)   (setf  joy-x  (-  joy-x  delta-x))) 
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(equal  key-value  '#\r)  (setf  joy-y  (-  joy-y  delta-y) ) ) 
(equal  key-value  '#\1)  (setf  joy-y  (+  joy-y  delta-y))) 
(equal  key-value  '#\=)   (setf  joy-r  (-  joy-r  delta-r) ) ) 
(equal  key-value  ' #\-)   (setf  joy-r  (+  joy-r  delta-r)))) 

(cond  ( (>=  joy-x  2)   (setf  joy-x  2)) 

(<=  joy-x  -2)  (setf  joy-x  -2))) 

(cond  ( (>=  joy-y  1)   (setf  joy-y  1) ) 

(<=  joy-y  -1)  (setf  joy-y  -1) ) ) 

(cond  ( (>=  joy-r  0.1)  (setf  joy-r  0.1)) 

(<=  joy-r  -0.1)  (setf  joy-r  -0.1))) 

(cond  ((equal  key-value  '#\q)  (setf  joy-x  0) 
(setf  joy-y  0)  (setf  joy-r  0))) 

(list  joy-x  joy-y  joy-r  (equal  key-value  '#\x)))) 


(defmethod  (joystick  : reset) 
0 
(setf  joy-x  0) 
(setf  joy-y  0) 
(setf  joy-r  0)  ) 


(setf  terrain- joystick  (make-instance  ' joystick) ) 


******************************************************** 


terrain  flavor  definition 


******************************************************** 


(defflavor  terrain ( (terrain-data  (make-array  '(49  49)  : initial-element  0)) 
terrain-height-array  terrain-height-list  joystick 
(cursor-x)  (cursor-y)  (max-x)  (max-y) 
(radius  500)  (alpha  0)  (beta  0)) 
0 
: initable-instance-variables 
: get table -instance- variables) 


(defmethod  (terrain  :create) 


0 
(send  self  :initti) 
(send  self  rmodify) 
(my-print  "Now  use  joystick  to  control  the  robot.")) 


(defmethod  (terrain  :kill) 
0 
(kill-robot -terrain-windows) 
(restore-lisp-listener) ) 


(defmethod  (terrain  :initti) 

0 
;  globals  :  middle-of-screen,  eye-space 
(move-and-shape-lisp- listener) 
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(let  ( (array-dims  (array-dimensions  terrain-data) ) ) 

(setf  radius  500  alpha  0  beta  0) 

(setf  max-x  (first  array-dims) ) 

(setf  max-y  (second  array-dims) ) 

(setf  cursor-x  (floor  (/  max-x  2))) 

(setf  cursor-y  (floor  (/  max-y  2)))) 
(setf  terrain-height-array  (make-array  (+  max-x  1) ) ) 
(make- robot -window) 
(setf  middle-of-screen 

(middle-of-robot-window) ) 
(setf  eye-space  (eye-trans  (list  500  0  0))) 
(send  self  : input-terrain-parameters) 

(my-print  "Please  use  joystick  to  transform  the  terrain.") 
(my-print  "Wait.") 
(make-visible) 

(send  self  :erase-obstacles) 
(my-print  "Now  you  can  translate  the  terrain.")) 


(defmethod  (terrain  :modify) 

() 
;  external  :  eye-space 
(do  ( (delta  0.0001) 

(joystick-value  nil) 
(end-flag  nil) ) 
(end-flag  (my-print  "Wait.") 

(send  joystick  : reset) 
(send  self  : save-terrain  eye-space) 
(send  self  : draw-obstacles) ) 
(make-visible) 

(setf  joystick-value  (send  joystick  :get- joy-value) ) 
(let  ( (x  (first  joystick-value) ) 
(y  (second  joystick-value) ) 
(r  (third  joystick-value) ) 
(fire  (fourth  joystick-value) ) ) 
(send  self  :era3e-terrain) 
(cond 

(fire  (cond  (  (user-ok) 

(cond  ( (user-save) 

(send  self  :save-terrain-to-disk  (user-file-name)))) 
(setf  end-flag  t) ) 
(t 
(send  joystick  : reset) 

(setf  joystick-value  (send  joystick  :get- joy-value) ) ) ) ) 
( (>  x  delta)      (setf  alpha  (+  alpha  0.05)) 
( (<  x  (-  delta))  (setf  alpha  (-  alpha  0.05)) 
( (>  y  delta)      (setf  beta   (+  beta   0.05)) 
( (<  y  (-  delta))  (setf  beta   (-  beta   0.05)) 
( (>  r  delta)      (setf  radius  (+  radius  10)) 
( (<  r  (-  delta))   (setf  radius  (-  radius  10)))) 
(setf  eye-space  (eye-trans  (list  radius  alpha  beta) ) ) 
(send  self  :draw-terrainl  eye-space)))) 


(defmethod  (terrain  : in-side-of-whole-terrain) 
(a-pos) 
(let  ( (dimension-terrain  (array-dimensions  terrain-data) ) 
(i-x  (floor  (first  a-pos) ) ) 
(i-y  (floor  (second  a-pos) ) ) ) 
(cond  ( (<  i-x  0)  nil) 
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(  (<  i-y  0)  nil) 

( (>  i-x  (-  (first   dimension-terrain)  1))  nil) 

( (>  i-y  (-  (second  dimension-terrain)  1))  nil) 


(T)) 


)) 


(defmethod  (terrain  :permitted-cell) 
(terrain-pos) 
(let  ( (i-x  (floor  (first  terrain-pos) ) )  ;  find  terrain  index 
(i-y  (floor  (second  terrain-pos) ) ) ) 
(if  (send  self  : in-side-of-whole-terrain  terrain-pos) 

(if  (equal  (aref  terrain-data  i-x  i-y)  0)  ;  permitted 
t 
nil)))) 


(defmethod  (terrain  :terrain-point) 
(a-pos-wrt-earth) 
(let*  ( (x  (first  a-pos-wrt-earth) ) 
(y  (second  a-pos-wrt-earth) ) 
(z  (send  self  :get-height  (list  x  y) ) ) ) 
(list  x  y  z) ) ) 


(defmethod  (terrain  : get-height) 

(a-pos-wrt-earth) 
;  range  0  =<  x  <=  (first  dimension-terrain-height),  (0  <  x  <39) 
;        0  =<  y  <=  (second  dimension-terrain) . 

(let*  ( (dimension-terrain-height  (array-dimensions  terrain-height-array) ) 
(x-min  0)  (x-max  (first  dimension-terrain-height)  ) 
(x  (first  a-pos-wrt-earth) ) ) 
(if  (or  (<  x  x-min)  (>  x  x-max) ) 
-1000 

(let*  ((i-x  (floor  x) )  ;  get  terrain  x-index 

(xl  (if  (<  (-  x  i-x)  0.5)  (-  i-x  1)  i-x)) 
(x2  (if  (<  (-  x  i-x)  0.5)  i-x  (+  i-x  1))) 
(xl  (if  (<  xl  x-min)  0  xl) ) 
(x2  (if  (>=  x2  x-max)  (-  x-max  1)  x2)) 
(zl  (aref  terrain-height-array  xl) ) 
(z2  (aref  terrain-height-array  x2) ) 
(slope  (-  z2  zl) ) 
(del-x  (-  x  xl) ) ) 
(+  zl  (*  slope  del-x) ) ) ) ) ) 


(setf  graph-terrain  (make-instance  'terrain 

: joystick  terrain- joystick) ) 


it***-****-***-******-***********************-)!***************** 


;   terrain . input-terrain-parameters 
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********************************************************** 


(defmethod  (terrain  : input-terrain-parameters) 

0 

(initialize -menu- variables) 

(cond  (  (setf  *old-terrain-f ile-name*  (get-old-terrain-file-name) ) 

(send  graph-terrain  : read-terrain-f rom-disk  *old-terrain-f ile-name*) ) 
(t 

(send  self  :get-new-terrain) ) ) ) 


(defmethod  (terrain  : get-new-terrain) 
0 
(send  self  :get-new-terrain-height) 
(send  self  :draw-terrain  eye-space) 
(send  self  : set-new-terrain-obstacles) 
(send  self  : set-new-ditch) ) 


(defmethod  (terrain  : get-new-terrain-height) 
0 
(let  (  (slope-type  (get-terrain-slope-type) ) 
(angle  nil)  (data  nil) ) 
(cond  ((equal  slope-type  'single-angle) 

(setf  angle  (get-terrain-slope-angle) ) ) 
((equal  slope-type  'manual) 
(setf  data  (get-terrain-slope-data) ) ) ) 
(my-print  "Wait.") 
(send  self  : read-terrain-height  slope-type  angle  data))) 


(defmethod  (terrain  : set-new-terrain-obstacles) 
0 
(let  ( (terrain-type  (get-terrain-obstacle-type) ) 
(values  nil) 

(obstacle-ratio  nil)  (random-seed  nil) ) 
(cond  ((equal  terrain-type  'random) 

(setf  values  (get-terrain-random-data) ) 
(setf  obstacle-ratio  (first  values) ) 
(setf  random-seed  (second  values) ) 
(my-print  "Wait . " ) 

(send  self  : random-terrain  obstacle-ratio  random-seed)) 
(t 
(send  self  :display-cursor) ) ) ) ) 


(defmethod  (terrain  : set-new-ditch) 
0 
(let  ((ditch-type  (get-ditch-type)) 
(width-location  nil) ) 
(cond  ((equal  ditch-type  'add-ditch) 

(setf  width-location  (get-ditch-width-location)  ) 

(my-print  "Wait . " ) 

(send  self  : add-ditch  (first  width-location) 

(second  width-location) ) 
(send  self  : draw-obstacles) 
(make-visible) ) 
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(t  nil) ) ) ) 


.a********************************************************* 

terrain .display-terrain 
.••A******************************************************* 


(defmethod  (terrain  :display-cursor) 
() 
(send  self  :make-all-permitted) 

(do  ((joy-data  nil)  (x  nil)  (y  nil)  (r  nil)  (fire  nil) 
(exit-flag  nil) ) 
(exit-flag  (send  self  :erase-cursor  (list  cursor-x  cursor-y) ) ) 
(make- visible) 

(setf  joy-data  (send  joystick  :get- joy-value) ) 
(setf  x  (-  (second  joy-data) ) )  (setf  y  (first  joy-data) ) 
(setf  r  (third  joy-data) )  (setf  fire  (fourth  joy-data) ) 
(send  self  :erase-cursor   (list  cursor-x  cursor-y) ) 
(cond 

(fire  (setf  exit-flag  t) ) 

((>  x  0)  (setf  cursor-x  (+  cursor-x  1))  (if  (>  cursor-x  max-x) 

(setf  cursor-x  max-x) ) ) 
( (<  x  0)  (setf  cursor-x  (-  cursor-x  1))  (if  (<  cursor-x  0) 

(setf  cursor-x  0))) 
( (>  y  0)  (setf  cursor-y  (+  cursor-y  1) )  (if  (>  cursor-y  max-y) 

(setf  cursor-y  max-y) ) ) 
( (<  y  0)  (setf  cursor-y  (-  cursor-y  1))  (if  (<  cursor-y  0) 

(setf  cursor-y  0) ) ) 
( (<  r  0)  (setf  (aref  terrain-data  cursor-x  cursor-y)  1)) 
( (>  r  0)  (setf  (aref  terrain-data  cursor-x  cursor-y)  1))) 
(send  self  :draw-cursor  (list  cursor-x  cursor-y) ) 
(send  self  :draw-obstacles) 
(send  joystick  :reset))) 


(defmethod  (terrain  :draw-terrain) 

(eye-space) 
;  external  function:  \display . library\move-to-earth,  draw-to-earth 
(dotimes  (x  (+  max-x  1) ) 

(move-to-earth  (list  x  0  (aref  terrain-height-array  x) ) ) 
(draw-to-earth  (list  x  max-x  (aref  terrain-height-array  x) ) ) ) 
(dotimes  (y  (+  max-y  1) ) 

(move-to-earth  (list  0  y  0)) 
(dotimes  (x  (+  max-x  1) ) 

(draw-to-earth  (list  x  y  (aref  terrain-height-array  x) ) ) ) ) ) 


(defmethod   (terrain  :draw-terrainl) 

.(eye-space) 
;  external  function:  \display .library\move-to-earth,  draw-to-earth 
(do  (  (xs  (list  0  max-x)  (cdr  xs) ) 
(x  nil) ) 
( (null  xs) ) 
(setf  x  (car  xs) ) 

(move-to-earth  (list  x  0  (aref  terrain-height-array  x) ) ) 
(draw-to-earth  (list  x  max-x  (aref  terrain-height-array  x) ) ) ) 
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(do  {  (ys  (list  0  max-y)  (cdr  ys)) 
(y  nil) ) 
(  (null  ys) ) 
(setf  y  (car  ys) ) 
(move-to-earth  (list  0  y  0)  ) 
(dotimes  (x  (+  max-x  1)) 

(draw-to-earth  (list  x  y  (aref  terrain-height-array  x) ) ) ) ) ) 


(defmethod  (terrain  :erase-obstacles) 

0 
;  externals  :  terrain 

;  external  function:  \display . library\move-to-earth,  draw-to-earth 
(dotimes  (i  (first  (array-dimensions  terrain-data) ) ) 
(dotimes  (j  (second  (array-dimensions  terrain-data))) 
(cond  ((equal  1  (aref  terrain-data  i  j)) 
(move-to-earth  (list  i  j)) 
(erase-to-earth  (list  (+  i  1)  (+  j  1) ) ) 
(move-to-earth  (list  (+  i  1)  j)) 
(erase-to-earth  (list  i  (+  j  1)))))))) 


(defmethod  (terrain  :erase-terrain) 

0 

(clear-robot-window) ) 


(defmethod  (terrain  :make-all-permitted) 
0 
(dotimes  (i  max-x) 
(dotimes  (j  max-y) 

(setf  (aref  terrain-data  i  j)  0)))) 


(defmethod  (terrain  : read-terrain-height) 

(terrain-slope-type  terrain-slope-angle  terrain-slope-data) 
(cond  ((equal  terrain-slope-type  'default) 

(setf  terrain-height-list  '((19  0)  (25  1)  (35  1.5)))) 
((equal  terrain-slope-type  'single-angle) 
(let*  ((angle  (*  pi  (/  terrain-slope-angle  180))) 
(max  (*  20  (tan  angle)))) 
(setf  terrain-height-list 
(list  '  (20  0) 

(list  40  max) 
)))) 
(t  (setf  terrain-height-list  terrain-slope-data) )  ) 
(let*  ((xl  0)  (zl  0)  (a-pair)  (zz  0) 

(x2  (first  (car  terrain-height-list) ) ) 
(z2  (second  (car  terrain-height-list))) 
(slope  (/  (-  z2  zl)  (-  x2  xl) ) ) ) 
(setf  terrain-height-list  (cdr  terrain-height-list) ) 
(dotimes  (i  (+  max-x  1) ) 

(setf  zz  (+  (*  slope  (-  i  xl) )  zl)) 
(cond  ( (equal  x2  i) 
(setf  xl  x2) 
(cond  ( (setf  a-pair  (car  terrain-height-list) ) 

(setf  terrain-height-list  (cdr  terrain-height-list) ) 
(setf  x2  (first  a-pair) ) 
(setf  z2  (second  a-pair) ) 
(setf  zl  zz) 
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(setf  slope  (/  (-  z2  zl)  (-  x2  xl)))) 
(T  (setf  slope  0)  (setf  zl  zz) ) ) ) ) 
(setf  (aref  terrain-height-array  i)  zz) ) ) ) 


(defmethod  (terrain  : save-terrain) 
(eye-space) 
(send  self  :draw-obstacles) 
(send  self  :draw-terrain  eye-space) 
(save-ter rain-to-terrain-buffer) ) 

(defmethod  (terrain  : save-terrain-to-disk) 
(file-name) 
(with-open-f ile 
(out-file 

(merge-pathnames  file-name  "robot : kwak . robot . terrain-data   data")  rdirection  :outp 


) 


(setf  *print-array*  t) 

(print  terrain-data  out-file) 

(print  terrain-height-array  out-file) 

(print  radius  out-file) 

(print  alpha  out-file) 

(print  beta  out-file) 

(setf  *print-array*  nil) ) ) 


(defmethod  (terrain  : read-terrain-f rom-disk) 
(file-name) 
(with-open-f ile 
(input-file 

(merge-pathnames  file-name  "robot : kwak . robot .terrain-data; ")  rdirection  :input) 
(setf  *print-array*  t) 
(setf  terrain-data  (read  input-file) ) 
(setf  terrain-height-array  (read  input-file) ) 
(setf  radius  (read  input-file) ) 
(setf  alpha  (read  input-file) ) 
(setf  beta  (read  input-file) ) 
(setf  *print-array*  nil) ) ) 


********************************************************** 

terrain .display-cursor 
********************************************************** 


(defmethod  (terrain  : draw-cursor 
(  position  ) 
(let*  ( (x  (first  position)) 
(y  .(second  position)  ) 
(pi  (list  (+  x  0.2)  (+ 
(p2  (list  (+  x  0.8)  (  + 
(p3  (list  (+  x  0.8)  (+ 
(p4  (list  (+  x  0.2)  (+ 
(points  (list  p2  p3  p4  pi))) 
(move-to-earth  pi) 
(do  ((points  points  (cdr  points))) 


y 

0 

.2) 

0)) 

y 

0 

.2) 

0)) 

y 

0 

8) 

0)) 

y 

0 

.8) 

0)) 

y 

0 

.2) 

0)) 

y 

0 

.2) 

0)) 

y 

0 

.8) 

0)) 

y 

0 

.8) 

0)) 
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((null  points)  'done-draw-cursor) 
(draw-to-earth  (car  points))))) 


(defmethod  (terrain  :draw-obstacles) 
() 
(dotimes  (i  max-x) 
(dotimes  (j  max-y) 

(cond  ((equal  1  (aref  terrain-data  i  j)) 
(move-to-earth 

(list  i  j  (aref  terrain-height-array  i) ) ) 
(draw-to-earth 

(list  (+  i  1)  (+  j  1)  (aref  terrain-height-array  (+  i  1) ) ) ) 
(move-to-earth 

(list  (+  i  1)  j  (aref  terrain-height-array  (+  i  1) ) ) ) 
(draw-to-earth 

(list  i  (+  j  1)  (aref  terrain-height-array  i) )))))) ) 


(defmethod  (terrain  :erase-cursor) 
(  position  ) 
(let*  ( (x  (first  position) ) 
(y  (second  position) ) 
(pi  (list  (+  x  0.2)  (+ 
(p2  (list  (+  x  0.8)  (+ 
(p3  (list  (+  x  0.8)  (+ 
(p4  (list  (+  x  0.2)  (+ 
(points  (list  p2  p3  p4  pi) ) ) 
(move-to-earth  pi) 
(do  ( (points  points  (cdr  points) ) ) 

((null  points)  ' done-erase-cursor) 
(erase-to-earth  (car  points) ) ) ) ) 


(defmethod  (terrain  : random-terrain) 

(obstacle-ratio  random-seed) 
(let  ((a  43411)  (b  17)  (c  640001)  (percent  nil)  (seed  nil)  (x  nil) 
(setf  percent  obstacle-ratio) 
(setf  seed  random-seed) 
(setf  x  seed) 
(dotimes  (i  max-x) 

(dotimes  (j  max-y) 
(if  (<  (/  (setf  x  (mod  (+  (*  a  x)  b)  c) )  c)  (/  percent  100)) 
(setf  (aref  terrain-data  i  j)  1))))) 
(send  self  :draw-obstacles) ) 


(defmethod  (terrain  :add-ditch) 
(width  location) 
(dotimes  (i  width) 

(dotimes  (j  max-y)     ' 

(setf    .(aref   terrain-data    (+    i    location)     j)    1)  )  )  ) 


********************************************************** 
graph-vehicle    flavor   definition 
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.********************************************************** 

(def flavor  graph-vehicle ( (vehicle-points  (make-array  28)) 

(body-points  (make-array  10)) 
(polygons  (make-array  13) ) 
(numpolys  nil) 

(vertices  (make-array  100) ) ) 
0 
: init able-instance- variables) 


(defmethod  (graph-vehicle  :init-data) 
0 
(send  self  : read-vehicle-data) )   ;  read  data  from  disk 


(defmethod  (graph-vehicle  :display) 
(a-H  foot-positions) 
(clear-robot -window) 

(send  self  :body-pento-wrt-earth  a-H  foot-positions) 
(send  self  :draw-vehicle  vehicle-points) 
(copy-terrain-to-robot -window) 
(make-visible) ) 


(defmethod  (graph-vehicle  : read-vehicle-data) 
0 
global  variables  :  vehicle-points,  polygons,  numpolys,  vertices 
format  of  file  :  num-of-points     num-of-polygons 
(  num  a-vehicle-point)  .... 

(  num-of-vertices   vertices-number-of-a-polygon) . . . 
(let*  ((vehicle-file  (open  "exp3 :kwak . robot /vehicle .data" ) ) 
(numpts  (read  vehicle-file) ) 
(numvtces  0)  (a-polygon  nil)) 
(setf  numpolys  (read  vehicle-file) ) 
(dotimes  (i  numpts) 

(setf  (aref  vehicle-points  i)  (cdr  (read  vehicle-file) ) ) ) 
(dotimes  (i  10) 

(setf  (aref  body-points  i)  (aref  vehicle-points  i) ) ) 
(dotimes  (i  numpolys) 

(setf  a-polygon  (read  vehicle-file) ) 

(setf  (aref  polygons  i)  (list  numvtces  (car  a-polygon) ) ) 

(do  ( (a-polygon-vertices  (cdr  a-polygon)  (cdr  a-polygon-vertices) 

(j  0  (+  j  1))) 
( (null  a-polygon-vertices) ) 
(setf  (aref  vertices   (+  numvtces  j)) 

(-  (first  a-polygon-vertices)  1))) 
(setf  numvtces  (+  numvtces  (car  a-polygon)))) 
(close  vehicle-file) ) ) 


[setf  graph-asv  (make-instance  'graph-vehicle)) 


********************************************************** 
graph-vehicle .display 
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********************************************************** 


(defmethod  (graph-vehicle  :body-pento-wrt-earth) 

(  a-H  foot-positions  ) 
;  library  :  transform 

(let  ((si  0.6616)  (s2  0.945)  (s3  3.308)  (1  0.8133)  (m  1.0467) 
(hipx-list  '(5.1667  5.1667  0.0  0.0  -4.9167  -4.9167)) 
(hipx-list  '(6.0  6.0  0.0  0.0  -6.0  -6.0)) 
(hipy-list  '(1.62  -1.62  1.62  -1.62  1.62  -1.62)) 
(signl-list  '(1-1   1-1   1  -1)) 
(sign2-list  '(1   1   1   1-1-1))) 
(send  self  :transf orm-body-points  a-H  body-points) 
(do  (  (positions  foot-positions  (cdr  positions) ) 
(hipx-list  hipx-list  (cdr  hipx-list) ) 
(hipy-list  hipy-list  (cdr  hipy-list) ) 
(signl-list  signl-list  (cdr  signl-list) ) 
(sign2-list  sign2-list  (cdr  sign2-list) ) 
(i  0  (+  i  1)  )  ) 
(  (null  positions)  nil) 

(let*  ( (foot-pos  (car  positions) ) 

(hipx  (car  hipx-list) )  (hipy  (car  hipy-list) ) 
(signl  (car  signl-list) )  (sign2  (car  sign2-list) ) 
(px  (-  (first  foot-pos)  hipx) ) 
(py  (-  (second  foot-pos)  hipy) ) 
(pz  (third  foot-pos) ) 

(theta  (vehicle-theta  py  pz  m  signl) ) 
(dm  (vehicle-dm  px  sign2) ) 
(dl  (vehicle-dl  py  pz  m  1) ) 
(top-pos  nil)  (knee-pos  nil) ) 
(setf  top-pos 

(transform  a-H 

(vehicle-top-pos  hipx  hipy  m  1  dl  theta  signl) ) ) 
(setf  knee-pos 

(transform  a-H 

(vehicle-knee-pos  hipx  hipy  m  1  si  s2  s3 
dl  dm  theta  signl  sign2) ) ) 
(setf  foot-pos  (transform  a-H  foot-pos) ) 
(setf  (aref  vehicle-points  (+  10  (*  3  i) ) ) 

top-pos) 
(setf  (aref  vehicle-points  (+  11  (*  3  i) ) ) 

knee-pos) 
(setf  (aref  vehicle-points  (+  12  (*  3  i) ) ) 
foot-pos) ) ) ) ) 


(defmethod  (graph-vehicle  :draw-vehicle) 

(  vehicle-point3  ) 
;  global  variables  :   polygons,  numpolys,  vertices 
(dotimes  (i  numpolys) 

(let  ((start  (first  (aref  polygons  i) ) ) 

(num-vertices  (second  (aref  polygons  i) ) ) ) 
(move-to-earth  (aref  vehicle-points 
(aref  vertices  start))) 
(dotimes  (j  num-vertices) 

(draw-to-earth-d  (aref  vehicle-points 

(aref  vertices  (+  start  j))))) 
))) 
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********************************************************** 

graph-vehicle .display .body-pento-wrt-earth 
********************************************************** 


(defmethod  (graph-vehicle  : transform-body-points) 

(a-H  body-points) 
;  globals  :  vehicle-points 
;  library  :  transform 
(dotimes  (i  10) 

(setf  (aref  vehicle-points  i) 

(transform  a-H  (aref  body-points  i) ) ) ) ) 


(defun  vehicle-dl  (py  pz  m  1) 

(/  (-  (sqrt  (+  (*  py  py)  (*  pz  pz)  (-  (*  m  m) ) ) )  1) 
4)) 


(defun  vehicle-dm  (px  sign2) 
(*  sign2  (/  px  5))) 


(defun  vehicle-knee-pos  (hipx  hipy  m  1  si  s2  s3 
dl  dm  theta  signl  sign2) 
(let*  ( (numer  (+  (*  si  si)  (-  (*  s2  s2))  (*  dl  dl)  (*  dm  dm) ) ) 
(denom  (*  2  si  (sqrt  (+  (*  dl  dl)  (*  dm  dm))))) 
(beta  (acos  (/  numer  denom) ) ) 
(alpha  (-  (/  pi  2)  (atan  dm  dl)  beta)) 
(sina  (sin  alpha) )  (cosa  (cos  alpha) ) 
(sint  (sin  theta) )  (cost  (cos  theta) ) 
(temp  (-  (*  s3  sina)  (-  dl  1))) 
(xk  (+  (*  sign2  s3  cosa)  hipx)) 

(yk  (+  (*  signl  (+  (*  temp  sint)  (*  m  cost)))  hipy)) 
(zk  (-  (  +  (*  temp  cost)  (*  m  sint))))) 
(list  xk  yk  zk) ) ) 


(defun  vehicle-theta  (py  pz  m  signl) 

(let*  ((anglel  (atan  (*  signl  py)  (*  -1  pz) ) ) 
(angle2  (atan  m  (sqrt  (+  (*  py  py) 
(*  pz  pz) 
(-  (*  m  m) )))))) 
(-  anglel  angle2) ) ) 


(defun  vehicle-top-pos  (hipx  hipy  m  1  dl  theta  signl) 
(let*  ( (xt  hipx) 
(1-dl  (-  1  dl) ) 
(sina  (sin  theta) ) 
(cosa  (cos  theta) ) 

(yt  (+  (*  signl  (+  (*  m  cosa)  (*  1-dl  sina)))  hipy)) 
(zt  (-  (*  m  sina)  (*  1-dl  cosa)))) 
(list  xt  yt  zt) ) ) 
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;;;  -*-  Mode : Common-Lisp;  Base: 10  -*- 


******************************************************************* 


ditch-robot  definition 


******************************************************************* 


(defflavor  ditch-robot  () 

(test -over lap-robot) 
) 


(defmethod  (ditch-robot  :initti) 
0 
(send  graph-asv  :init-data) 

(setf  vision-system  (make-instance  'ditch-vision-system  :owner  self)) 
(send  vision-system  :initti) 
(setf  joystick  (make-instance  'joystick)) 
(send  joystick  : reset) 
(empty-queue  lift-queue) 
(setf  lift-flag  t) 
(let  ((H)) 

(setf  body  (make-instance  'stop-body  :owner  self)) 
(setf  H  (send  body  :initti)) 
(setf  legs  (list 

(make-instance  'test-overlap-leg  :name  ' legl  : owner  self) 
(make-instance  'test-overlap-leg  : name  ' leg2  :owner  self) 
(make-instance  'test-overlap-leg  :name  ' leg3  rowner  self) 
(make-instance  'test-overlap-leg  :name  ' leg4  :owner  self) 
(make-instance  'test-overlap-leg  :name  ' leg5  :owner  self) 
(make-instance  'test-overlap-leg  :name  ' leg6  : owner  self) 
)) 
(mapcar  #' (lambda  (a-leg)  (send  a-leg  :initti  H) )  legs)) 
) 


(defmethod  (ditch-robot  :at-stability-limit) 
0 
(not  (send  self  :stable))) 


(defmethod  (ditch-robot  : stop-motion) 
0 
(send  body  : stop-body-motion) ) 


(defmethod  (ditch-robot  : resume-motion) 
0 
(send  body  : restore-body-motion) 
t) 

i 
******************************************************** 


(defun  at_stability_limit  0 

(send  asv  : at-stability-limit) ) 
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(defun  stop_motion  () 

(send  asv  : stop-motion) ) 


(defun  resume_motion  () 

(send  asv  : resume-motion) ) 


ditch-vision-t .lisp         Thu  Nov  29  11:28:50  1990 
; ; ;  -*-  Mode : Common-Lisp;  Base: 10  -*- 


******************************************************************* 


ditch-vision-system  definition 


******************************************************************* 


(defflavor  ditch-vision-system  () 
(vis ion -system) 

) 


(defmethod  (ditch-vision-system  :on-ditch-area) 
(body-HIO) 
(let  ((x  (aref  body-HIO  0  3))) 
(cond  ((and  (>=  x  (-  21  7)) 

(<-  x  (+  21  *ditch-width*) ) ) 
t) 
(t  nil)))) 


(setf  *ditch-width*  6) 
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;;;  -*-  Mode : Common-Lisp;  Base: 10  -*- 


***************************************************************** 


executor  flavor  definition 


***************************************************************** 


(defflavor  executor 

(leg-pos-wrt-body  desired-f oothold-pos-wrt -earth 

time  command  owner  sensor  (lift-height  1.4) 
(Tl  0.6)   (T2  1.0)   (T3  0.4)   (T4  0.6) 
(planned-contact-time  0.4)  self-time 
(sampling-time  0.1)  ready-pos 

HI  inv-Hl  body-trans-ratel  body-rotate-ratel) 
0 
: init able -instance -variables) 


(defmethod  (executor  : set-desired-pos) 
(a-pos) 
(setf  desired-f oothold-pos-wrt-earth  a-pos) ) 


(defmethod  (executor  :get-desired-pos) 
0 
desired-f oothold-pos-wrt -earth) 


(defmethod  (executor  : send-command) 
(a-command) 
(setf  command  a-command) ) 


(defmethod  (executor  : set-time) 
(a-time) 
(setf  time  a-time) ) 


(defmethod  (executor  : leg-pos-wrt-body) 
0 
leg-pos-wrt-body) 


(defmethod  (executor  :move) 

(H  inv-H  body-trans-rate  body-rotate-rate) 
(setf  HI  H) 

(setf  inv-Hl  inv-H)      ' 
(setf  bodyrtrans-ratel  body-trans-rate) 
(setf  body-rotate-ratel  body-rotate-rate) 
(cond  ((equal  command  'ready) 

(send  self  :move-in-ready)  ) 
((equal  command  'advance) 

(send  self  :move-in-advance) ) 
((equal  command  'descent) 
(send  self  :move-in-descent) ) 
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((equal  command  'contact) 

(send  self  :move-in-contact ) ) 
((equal  command  'support) 

(send  self  :move-in-support) ) 
((equal  command  'lift) 

(send  self  :move-ift-lif t) ) 
((equal  command  'return) 

(send  self  :move-in-return) ) 
) 


(def method  (executor  :move-in-contact) 
0 
(let  ( (leg-velocity-wrt-body  (send  self  :f ind-velocity-wrt-body) ) ) 
(setf  leg-pos-wrt-body 

(vectadd  (magvect  sampling-time  leg-velocity-wrt-body) 
leg-pos-wrt-body) ) ) ) 


(defmethod  (executor  : f ind-velocity-wrt-body) 

0 
;  returns  f oot-velocity-wrt-body 

;  velocity  =  -  (  body-trans-rate  +  body-rotate-rate  X  leg-pos  ) 
;  globals  v  :  body-trans-ratel,  body-rotate-ratel 
;  lib  :  vectsub,  vectadd,  crossprod 
(vectsub  ' (0  0  0) 

(vectadd  body-trans-ratel 

(crossprod  body-rotate-ratel  leg-pos-wrt-body) ) ) 


(defmethod  (executor  :move-in-advance) 
0 
(let  ( (desired-pos  (send  self  :desired-advance-pos-wrt-body)  ) 
(dt  (-  Tl  time) ) ) 
(send  self  :move-del  desired-pos  leg-pos-wrt-body  dt) ) 
(setf  self-time  0.0)) 


(defmethod  (executor  :desired-advance-pos-wrt-body) 

0 
;  a-pos  is  desired-stepping-pos-wrt -earth 
;  returns  desired-pos-wrt-body  in  deploy  state 
;  global  variable  :  HI,  inv-Hl 

;  global  function  :  to-earth-transf orm,  to-body-transf orm,  f ind-terrain-hegiht 
(let*  ( (desired-pos-wrt-earth  desired-foothold-pos-wrt-earth) 

(terrain-height  (third  (send  owner  : terrain-point  desired-pos-wrt-earth))) 
(desired-pos-height-wrt-earth  (+  terrain-height  lift-height) ) 
(pos-wrt -earth  (list  (first   desired-pos-wrt-earth) 

(second  desired-pos-wrt-earth) 
desired-pos-height-wrt-earth) ) ) 
(to-body-transform  inv-Hl  pos-wrt -earth) ) ) 


(defmethod  (executor  :move-in-descent) 

0 
;  global  function  :  to-body-transform 
;  global  variables  :  inv-Hl 

(let  ( (dt  (-  planned-contact-time  self-time) ) ) 
(if  (<  dt  0.05) 
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(setf  leg-pos-wrt-body  (to-body-transf orm 

inv-Hl  desired-f oothold-pos-wrt-earth) ) 
(send  self  :rnove-del 

(to-body-transf orm  inv-Hl  desired-f oothold-pos-wrt-earth) 
leg-pos-wrt-body  dt) ) ) 
;   ) 


; (defmethod  (executor  :move-in-descent  : after) 
0 
(setf  self-time  (+  self-time  sampling-time) ) ) 


(defmethod  (executor  :move-del) 

(desired-pos  present-pos  dt) 
;  set  new  leg-pos  depending  on  the  arguments 
;  lib  :  vectadd,  magvect 
(if  (<  dt  0.05) 

(setf  leg-pos-wrt-body  desired-pos) 
(let*  ( (inv-time-dif f  (/  1  dt) ) 

(del  (vectsub  desired-pos  present-pos) ) 
(velocity  (magvect  inv-time-dif f  del) ) ) 
(setf  leg-pos-wrt-body 

(vectadd  present-pos  (magvect  sampling-time  velocity) ) ) ) ) ) 


(defmethod  (executor  :move-in-lif t) 
0 
(let*  ( (dt  (-  T3  time) ) 

(desired-pos  (send  self  : lif t-pos-desired) ) 
(z  (third  desired-pos) ) ) 
(send  self  :move-del  desired-pos  leg-pos-wrt-body   dt) 
(setf  ready-pos 

(list  (first  ready-pos)  (second  ready-pos)  z) ) ) ) 


(defmethod  (executor  : lif t-pos-desired) 

;  returens  position-wrt-body  which  will  be  at  the  end  of  lift  state. 
;  global  f  :  to-body-transf orm, 
;  global  v  :  inv-Hl 
0 
(let*  ( (leg-pos-wrt-earth  (to-earth-transf orm  HI  leg-pos-wrt-body) ) 

(desired-height  (+  lift-height  (third  (send  owner  :terrain-point  leg-pos-wrt-ee 
h))))) 

(to-body-transform  inv-Hl  (list  (first   leg-pos-wrt-earth) 

(second  leg-pos-wrt-earth) 
de ;,i red-height)  )  )  ) 


(defmethod  (executor  :move-in-ready) 
0  ' 

(setf  leg-pos-wrt-body  ready-pos) ) 


(defmethod  (executor  :move-in-return) 

0 
;  Modifying  leg-pos-z  is  redundent  but  it  can  correct  disturbance  by  itself 
(let  ( (dt  (-  T4  time) ) 
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(desired-pos  ready-pos) ) 
(send  self  :move-del  desired-pos  leg-pos-wrt-body  dt))) 


(defmethod  (executor  :move-in-support) 

0 
;  globals  :  body-trans-ratel,  body-rotate-ratel 
;  lib  :  vectadd,  magvect 

;  In  general  terrain,  leg-pos-z  should  be  updated  by  real  terrain  height 
(let  ( (leg-velocity-wrt-body  (send  self  :f ind-velocity-wrt-body) ) ) 
(setf  leg-pos-wrt-body 

(vectadd  (magvect  sampling-time  leg-velocity-wrt-body) 
leg-pos-wrt-body) ) ) ) 


((x 

(y 

(z 

(cond  ( 


(defmethod  (executor  rinitti) 

(leg-name  init-H) 
(setf  sensor  (send  owner  : contact-sensor) ) 
(let  ((x  (aref  init-H  0  3)) 

(aref  init-H  1  3) ) 

(aref  init-H  2  3))) 

(equal  leg-name  'legl) 

(setf  ready-pos  ' (  5   3  -4)) 

(setf  leg-pos-wrt-body  (list   6   3  (-  z) ) ) 

(setf  desired-foothold-pos-wrt -earth  (list  (+  x  6) 
((equal  leg-name  'leg2) 

(setf  ready-pos  ' (  5  -3  -4)) 

(setf  leg-pos-wrt-body  (list   5-3  (-  z) ) ) 

(setf  desired-f oothold-pos-wrt-earth  (list 
((equal  leg-name  'leg3) 

(setf  ready-pos  ' (  0   3  -4) ) 

(setf  leg-pos-wrt-body  (list   0   3  (-  z) ) ) 

(setf  desired-foothold-pos-wrt-earth  (list 
((equal  leg-name  'leg4) 

(setf  ready-pos  ' (  0  -3  -4) ) 

(setf  leg-pos-wrt-body  (list   0-3  (-  z) ) ) 

(setf  desired-foothold-pos-wrt-earth  (li3t 
((equal  leg-name  'leg5) 

(setf  ready-pos  ' (-5   3  -4) ) 

(setf  leg-pos-wrt-body  (list  -5   3  (-  z) ) ) 

(setf  desired-foothold-pos-wrt-earth  (list  (-  x 
((equal  leg-name  'leg6) 

(setf  ready-pos  ' (-5  -3  -4) ) 

(setf  leg-pos-wrt-body  (list  -5  -3  (-  z) ) ) 

(setf  desired-foothold-pos-wrt-earth  (list  (-  x  5)  (-  y  3)  0) ) ) ) ) 
) 


(+  y  3)  0) ) ) 


(+  x  5)  (-  y  3)  0))  ) 


(+  x  0)  (+  y  3)  0)  )) 


(+  x  0)  (-  y  3)  0))) 


5)  (+  y  3)  0)) ) 
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; ; ;  -*-  Mode : Common-Lisp;  Base: 10  -*- 


******************************************************************* 


load  file  (f-load-t-444 . lisp) 


******************************************************************* 


;  graph-terrain  is  used  in  sensor  and  vision 

;  Overlapped  working  volume  (1  foot) 

;  Front  and  rear  are  not  extended. 

;  logic  change 


(load  "robot : kwak . robot ;math-t" ) 

(load  "robot : kwak . robot ; user-interf ace-t2" ) 

( load  " robot : kwak . robot ; graph-t 1 " ) 

(load  "robot :kwak. robot ;display-t2") 

(load  "robot : kwak . robot 1 ; vision-t" ) 

(load  "robot :kwak. robot 5; ditch- vision-t") 

( load  " robot : kwak . robot ; t km-t " ) 

(load  "robot :kwak . robot 4; overlap-tkm-t") 

( load  "robot : kwak . robot 1 ; f oothold-t " ) 

(load  "robot : kwak . robot4 ; overlap-f oothold-t" ) 

(load  "robot : kwak . robot ; sensor-t" ) 

(load  "robot : kwak . robotl;executor-t") 

(load  "robot :kwak . robot ;control-machine-t") 

(load  "robot :kwak. robot ;plan-machine-t") 

( load  " robot : kwak . robot 1 ; leg-t " ) 

(load  "robot :kwak. robot 4; over lap- leg-t") 

(load  "robot :kwak. robot 5; test-overlap-leg-t-441") 

( load  " robot : kwak . robot ; st ability-t2 " ) 

(load  "robot : kwak. robot ; support -plane-t") 

(load  "robot :kwak. robot ;h-calculator-t") 

(load  "robot :kwak. robot ; command- regulator-t") 

(load  "robot :kwak. robot ;terrain-regulator-t") 

(load  "robot : kwak. robot ;body-controller-t") 

(load  "robot :kwak. robot l;body-t") 

(load  "robot : kwak. robot 6 ; stop-body-t") 

( load  " robot : kwak . robot 1 ; robot -t 1 " ) 

( load  " robot : kwak . robot 4 ; overlap-robot -t " ) 

(load  "robot : kwak. robot 5; test-overlap-robot-t-4 42") 

(load  "robot :kwak. robot6;ditch-robot-t") 

( load  " robot : kwak . robot  6 ; robot 4  44") 

( load  "robot : kwak . robot ; add-to-system-menu" ) 

(setf  asv  (ma'ke-instance  'ditch-robot)) 
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;  ;  -*-  Mode : Common-Lisp;  Base: 10  -*- 
******************************************************************* 

foothold-finder  definition 
******************************************************************* 


(def flavor  foothold-finder (sixteen-f ootholds 

four-lines  tkm-calculator 
(no-cell-available-flag  nil) 
(TKM-margin  0.4)  owner) 
0 
:  initable-instance-variables) 


(defmethod  (foothold-finder  :initti) 
(leg-name) 
(cond  ((equal  leg-name  'legl) 
(setf  sixteen-f ootholds 


((  7.3  4.3)  (  7.3  3.3)  (  7.3  2.3)  (  7.3  1.3) 

(  6.3  4.3)  (  6.3  3.3)  (  6.3  2.3)  (  6.3  1.3) 

(  5.3  4.3)  (  5.3  3.3)  (  5.3  2.3)  (  5.3  1.3) 

(  4.3  4.3)  (  4.3  3.3)  (  4.3  2.3)  (  4.3  1.3))) 


(setf  four-lines 

'  ( ( (0   0.3420  -0. 

( (0  -0.3420  -0. 

( (0  -0.3420  -0. 

( (0   0.3420  -0. 

((equal  leg-name  'leg2) 

(setf  sixteen-f ootholds 

'  ((  7.3  -4.3)  (  7 

(  6.3  -4.3)  (  6 

(  5.3  -4.3)  (  5 

(  4.3  -4.3)  (  4 

(setf  four-lines 

'  (( (0   0.3420  -0. 

( (0  -0.3420  -0. 

( (0  -0.3420  -0. 

( (0   0.3420  -0. 

((equal  leg-name  'leg3) 

(setf  sixteen-footholds 


' ((  1.5 
(  0.5 
(-0.5 
(-1.5 


3) 

,3) 

3) 

3) 


(  1 
(  0 
(-0 
(-1 


(setf  four-lines 

'  ( ((0   0.3420  -0. 

((0  -0.3420  -0. 

( (0  -0.3420  -0. 

( (0   0.3420  -0. 

((equal  leg-name  'leg4) 

(setf  sixteen-footholds 

'  ((  1.5  -4.3)  (  1 

(  0.5  -4.3)  (  0 

(-0.5  -4.3)  (-0 

(-1.5  -4.3)  (-1 

(setf  four-lines 

'  ( ( (0  0.3420  -0. 
( (0  -0.3420  -0. 
( (0  -0.3420  -0. 
( (0   0.3420  -0. 


9397)  (  8.0832  2.7339  0)) 

9397)  (  8.0832  2.7339  0) ) 

9397)  (  3.4167  2.7339  0) ) 

9397)  (  3.4167  2.7339  0) ) ) ) ) 


.3  -3.3)  (  7.3  -2.3)  (  7.3  -1.3) 

.3  -3.3)  (  6.3  -2.3)  (  6.3  -1.3) 

.3  -3.3)  (  5.3  -2.3)  (  5.3  -1.3) 

.3  -3.3)  (  4.3  -2.3)  (  4.3  -1.3) ) ) 

9397)  (  8.0832  -2.7339  0) ) 

9397)  (  8.0832  -2.7339  0) ) 

9397)  (  3.4167  -2.7339  0) ) 

9397)  (  3.4167  -2.7339  0) ) ) ) ) 


.5   3.3)  (  1.5  2.3)  (  1.5   1.3) 

.5   3.3)  (  0.5  2.3)  (  0.5   1.3) 

.5   3.3)  (-0.5  2.3)  (-0.5   1.3) 

.5   3.3)  (-1.5  2.3)  (-1.5   1.3))) 

9397)  (  2.2915  2.7339  0) ) 

9397)  (  2.2915  2.7339  0) ) 

9397)  (-2.2915  2.7339  0) ) 

9397)  (-2.2915  2.7339  0))))) 


.5  -3.3)  (  1.5  -2.3)  (  1.5  -1.3) 

.5  -3.3)  (  0.5  -2.3)  (  0.5  -1.3) 

.5  -3.3)  (-0.5  -2.3)  (-0.5  -1.3) 

.5  -3.3)  (-1.5  -2.3)  (-1.5  -1.3)) 

9397)  (  2.2915  -2.7339  0) ) 

9397)  (  2.2915  -2.7339  0) ) 

9397)  (-2.2915  -2.7339  0) ) 

9397)  (-2.2915  -2.7339  0) ) ) ) ) 
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((equal  leg-name  'leg5) 
(setf  sixteen-f ootholds 


' ( (-4.0   4.3) 

(-4.0   3. 

3)  (-4.0 

(-5.0   4.3) 

(-5.0   3. 

.3)  (-5.0 

(-6.0   4.3) 

(-6.0   3. 

,3)  (-6.0 

(-7.0   4.3) 

(-7.0   3, 

,3)  (-7.0 

(setf 

four-lines 

' ( ( (0   0.3420 

-0.9397) 

(-3.3332 

(  (0  -0.3420 

-0.9397) 

(-3.3332 

((0  -0.3420 

-0.9397) 

(-7.8332 

((0   0.3420 

-0.9397) 

(-7.8332 

((equal  leg-name  'leg6) 

(setf 

sixteen-f ootholds 

' ( (-4.0  -4.3) 

(-4.0  -3 

.3)  (-4.0 

(-5.0  -4.3) 

(-5.0  -3 

.3)  (-5.0 

(-6.0  -4.3) 

(-6.0  -3 

.3)  (-6.0 

(-7.0  -4.3) 

(-7.0  -3 

.3)  (-7.0 

(setf 

four-lines 

' ( ( (0   0.3420 

-0.9397) 

(-3.3332 

(  (0  -0.3420 

-0.9397) 

(-3.3332 

( (0  -0.3420 

-0.9397) 

(-7.8332 

((0   0.3420 

-0.9397) 

(-7.8332 

2.3)  (-4.0  1.3) 

2.3)  (-5.0  1.3) 

2.3)  (-6.0  1.3) 

2.3)  (-7.0  1.3))) 

2.7339  0) ) 
2.7339  0) ) 
2.7339  0) ) 
2.7339  0) ) ) ) ) 


-2.3)  (-4.0  -1.3) 

-2.3)  (-5.0  -1.3) 

-2.3)  (-6.0  -1.3) 

-2.3)  (-7.0  -1.3))) 

-2.7339  0)  ) 

-2.7339  0)  ) 

-2.7339  0)  ) 

-2.7339  0)) ) )) 


) 

(setf   tkm-calculator    (send  owner    : tkm-calculator)  ) 

) 


(defmethod  (foothold-finder  : find-foothold) 

(H6  inv-H6  body-trans-ratelO  body-rotate-ratelO 
est imated-support -plane) 
;  returns  ( (max-f oothold  max-tkm)  (foothold-list)  (tkm-list) ) 
;  all  points  are  wpt  body  coordinate  system, 
(let*  ( (estimated-support-plane-wrt-body 

(plane-transform  estimated-support -plane  H6) ) 
(four-points  (send  self 

: f our-points-on-support -plane 

four-lines  estimated-support -plane-wrt -body) ) 
(possible-footholds  (send  self 

: get-possible-footholds 
(send  self 

: estimate-footholds 

four-points  est imated-support -plane-wrt -body) 
H6  inv-H6) ) ) 
(send  self 

: get-f oothold-with-max-TKM 
possible-footholds  H6 
body-trans-ratelO  body-rotate-ratelO)  )  ) 


"•••a**************************************************' 


foothold-finder . find-foothold 


•I******************************************************" 
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(defmethod  (foothold-finder  :estimate-f ootholds) 

(four-points-wrt-body  estimated-support-plane-wrt-body) 
;  returns  estimate-f ootholds-wrt-body 

(do*  ( (footholds  sixteen-f ootholds  (cdr  footholds) ) 
(out-footholds  nil) 
(a-foothold  nil) ) 
( (null  footholds) 
(get-points-on-support-plane  out-footholds  estimated-support-plane-wrt-body) ) 
(setf  a-foothold  (car  footholds) ) 
(if  (in-side-of-polygon  a-foothold 

(pick-two-dimensions  four-points-wrt-body) ) 
(setf  out-footholds  (cons  a-foothold  out-footholds))))) 


(defmethod  (foothold-finder  : four-points-on-support -plane) 

(four-lines  est imated-support -plane- wrt -body) 
;  returns  four  points  which  are  intersected  by  four-lines  on 
;  est imated-support -plane -wrt -body 
;  math  lib:  plane-intersection 

(do*  (  (lines  four-lines  (cdr  lines) ) 
(points  nil) ) 
( (null  lines)  points) 
(setf  points  (cons  (plane-intersection  (car  lines) 

est imated-support -plane -wrt -body) 
points) ) ) ) 


(defmethod  (foothold-finder  :get-f oothold-with-max-TKM) 
(possible-footholds  H 

body-trans -rate  body-rot ate- rate) 
;  returns  ( (max-f oothold  max-tkm)  (foothold-list)  (tkm-list) ) 
;  sets  no-cell-available-flag 
;  real-footholds  is  really  possible  footholds 

(do  ( (footholds  possible-footholds  (cdr  footholds) ) 

(max-foothold  nil)  (a-foothold  nil)  (TKM-list  nil)  (a-TKM  nil) 
(real-footholds  nil)  (max-TKM  -100.0)) 
( (null  foothold3) 
(setf  no-cell-available-flag  (<  max-TKM  TKM-margin) ) 
(if  (>=  max-TKM  TKM-margin) 
(make-output -form 

max-foothold  max-TKM  real-footholds  TKM-list  H) 
nil)) 
(setf  a-foothold  (car  footholds) ) 
(setf  a-TKM  (send  tkm-calculator  :find-tkm 

a-foothold  body-trans-rate  body-rotate-rate) ) 
(if  a-TKM 

(progn  (setf  TKM-list  (cons  a-TKM  TKM-list)) 

(setf  real-footholds  (cons  a-foothold  real-footholds) ) 
(if  (>  a-TKM  max-TKM) 

(progn  (setf  max-TKM  a-TKM) 

(setf  max-foothold  a-foothold) )))))) 


(defmethod  (foothold-finder  :get-possible-f ootholds) 

(estimated-f ootholds  H  inv-H) 
;  returns  possible-footholds  wrt  body 
(to-body-transf orm  inv-H 

(send  self  :f ind-possible-f ootholds 

(to-earth-transf orm  H  estimated-f ootholds) 
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)  ) 
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»***********************•***********■**********•********" 


;   foothold-finder .estimate-foothold 


»  ****************************************•*************" 


(defun  check-polarity  (pointl  point2  point3) 
(let*  ( (vectl  (vectsub  point2  pointl)) 
(vect2  (vectsub  point3  pointl) ) ) 
(if  (not  (third  vectl)) 

(progn  (setf  vectl  (reverse  (cons  0 
(setf  vect2  (reverse  (cons  0 
(crossprod  vectl  vect2) ) ) 


(reverse  vectl)))) 
(reverse  vect2) ) ) ) ) ) 


(defun  get-points-on-support -plane  (points  estimated-support-plane-wrt-body) 
;  returns  intersection  points  with  support  plane  in  z-body  direction. 
;  math  lib:  plane-intersection 

(do*  ( (points  points  (cdr  points) ) 
(out-points  nil) ) 
( (null  points)  out-points) 
(setf  out-points  (cons  (plane-intersection 

(make-line-to-get -point -on-support -plane 

(car  points) ) 
estimated-support-plane-wrt-body)  out -points) ) ) ) 


(defun  in-side-of -polygon  (a-point  polygon-points) 

;  polygon-points  must  be  convext -polygon  and  in  order  &  two  dimensional  points, 
(do*  ((first-points   polygon-points  (cdr  first-points)) 
(second-points  (reverse  (cons  (car  first-points) 

(reverse  (cdr  first-points) ) ) ) 
(cdr  second-points) ) 
(signs  nil)  (first-point  nil)  (second-point  nil) ) 
((null  first-points)  (same-polarity  signs)) 
(setf  first-point   (car  first-points) ) 
(setf  second-point  (car  second-points)  ) 

(setf  signs  (cons  (check-polarity  first-point  second-point  a-point) 
signs) ) ) ) 


(defun  make-line-to-get-point-on-support-plane  (a-point) 

;  a-point  is  two  dimensional  point. 

;  returns  a-line  ( (z-direction)  (a-point  -100) ) 

(list  '(0  0  1)  (list  (first  a-point)  (second  a-point)  -100) ) ) 


(defun  pick-two-dimensions  (points) 
(if  (listp  (first  points) ) 

(do*  ( (points  points  (cdr  points) ) 
(a-point  nil) 
(out-points  nil) ) 


more  than  one  point  case 


foothold-t .lisp         Thu  Nov  29  11:29:51  1990         5 

((null  points)  out-points) 
(setf  a-point  (car  points) ) 

(setf  out-points  (cons  (list  (first  a-point)  (second  a-point) ) 

out-points) ) ) 
(list  (first  points)  (second  points) ) ) )   ;  one  point  case 


(defun  same-polarity  (signs) 

(do  ( (signs  (cdr  signs)  (cdr  signs) ) 

(first-sign  (plusp  (third  (car  signs) ) ) ) 
(same  T) ) 
((null  signs)  same) 
(if  (not  (equal  first-sign  (plusp  (third  (car  signs) ) ) ) ) 
(setf  same  nil) ) ) ) 


foothold-finder . f ind-f oothold.get-f oothold-with-MAX-tkm 
■I********************************************************" 


(defun  make-output-form 

(max-foothold  max-TKM  possible-footholds  TKM-list  H) 
output-form  :  ( (f cothold-with-max-tkm  tkm) 

(leg-pro jected-permitted-f oot ho Ids) 
(leg-pro jected-TKM-list)  ) 
output  footholds  are  in  earth  coordinate, 
math  lib  :  to-earth-transform 

(list  (list  (to-earth-transform  H  max-foothold)  max-TKM) 
(to-earth-transform  H  possible-footholds) 
TKM-list) ) 


I'******************************************************" 

foothold-finder . select-foothold. get-possible-foothold 

•••***************************•*************************'• 


(defmethod  (foothold-finder  : find-possible-footholds) 

(es timated-f oot ho Ids -wrt -earth) 
;  returns  possible-f ootholds-wrt -earth 
;  graph-terrain  is  object. 

(do*  ( (footholds  estimated-footholds-wrt-earth  (cdr  footholds) ) 
(a-foothold  nil)  (t-cell  nil)  (out-footholds  nil) ) 
((null  footholds)  (unique-f ootholds-only  out-footholds)) 
(setf  a-foothold  (car  footholds) ) 

(setf  t-cell  (get-center-of-digitized-terrain-cell  a-foothold) ) 
(setf  out-footholds 

(cons  (list  (+  (first  t-cell)  0.5) 
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(+  (second  t-cell)  0.5) 
0.0)  out-footholds)))) 

(if  (send  owner  :permitted-cell  t-cell) 
(setf  out-footholds 

(cons  (send  owner  : terrain-point  t-cell) 
out-footholds) ) ) ) ) 


(defun  get-center-of-digitized-terrain-cell  (a-foothold) 
;  cell  resolution  is  1  foot  by  1  foot 

(list  (+  (floor  (first   a-foothold))  0.5) 

(+  (floor  (second  a-foothold))  0.5))) 


(defun  unique-footholds-only  (mixed-footholds) 

(do*  ((footholds  mixed-footholds  (cdr  footholds)) 
(out-footholds  nil) 
(a-foothold  nil) ) 
((null  footholds) • out-footholds) 
(setf  a-foothold  (car  footholds) ) 
(if  (not  (member  a-foothold  out-footholds  :test  'equal)) 

(setf  out-footholds  (cons  a-foothold  out-footholds) ) ) ) ) 
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;;;  _*_  Mode : Common-Lisp;  Package :USER;  Base: 10  -*- 

.*******************************■**********•**************** 
;   low  level  graph  routines 


(defvar  *robot-display-window*  nil) 

(defvar  *robot-display-window-array*  nil) 

(defvar  *robot-window*  nil) 

(defvar  *robot-window-array*  nil) 

(defvar  *robot-window-width*  nil) 

(defvar  *robot-window-height*  nil) 

(defvar  *terrain-buf f er*  nil) 

(defvar  *terrain-buf f er-array*  nil) 

(defvar  *max-y*  nil) 

(defvar  *start-point*  nil) 

;TI 

(defvar  *xs*  (make-array  2)) 

(defvar  *ys*  (make-array  2)) 


(defun  copy-terrain-to-robot-window  () 
(tv: sheet -force-access  (* robot -window*) 
(send  *robot-window*  :bitblt 

tv:alu-ior  *robot-window-width*  *robot-window-height* 
*terrain-buf fer-array*  2  2  0  0))) 


(defun  draw-to  (a-point  a-window) 
;  global  variables  :  *start -point* 
(tv: sheet-force-access  (a-window) 

(send  a-window  ' :draw-line  (first  *start-point*) 
(-  *max-y*  (second  *start-point*) ) 
(first  a-point) 

(-  *max-y*  (second  a-point))    tv:alu-ior)) 
(setq  *start -point*  a-point) ) 


(defun  draw-to-d  (a-point  a-window) 
;  global  variables  :  *start -point* 
(tv: sheet-force-access  (a-window) 

(setf  (aref  *xs*  0)  (+  4  (first  *start -point*) ) ) 
(setf  (aref  *xs*  1)  (+  4  (first  a-point))) 

(setf  (aref  *ys*  0)  (+  4  (-  *max-y*  (second  *start -point*) ) ) ) 
(setf  (aref  *ys*  1)  (+  4  (-  *max-y*  (second  a-point) ) ) ) 
(send  a-window  : draw-wide-curve  *xs*  *ys*  2) ) 
(setq  *start -point*  a-point) ) 


i 
(defun  erase-to  (a-point  a-window) 
;  global  variables  :  *start-point* 
(tv: sheet -force-access  (a-window) 

(send  a-window  ' :draw-line  (first  *start -point*) 
(-  *max-y*  (second  *start-point*) ) 
(first  a-point) 

(-  *max-y*  (second  a-point) )    tv:alu-andca) ) 
(setq  *start -point*  a-point) ) 
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(defun  get-keyboard-input ( ) 

;  This  is  not  for  the  graphics,  but  this  function  uses  Zeta  LISP. 
;  This  is  the  reason  why  this  function  is  in  Zeta  graphic  package. 
(send  *terminal-io*  : tyi-no-hang) ) 


(defun  make-robot-window  () 

(setq  *robot-display-window*  (tv:make-window 

' tv: window 
:blinker-p  nil 
:position  ' (0  0) 
:width  *screen-width* 

•.height  (truncate  (*  0.8  *screen-height*)  ) 
:borders  2 

:  label  "robot-display-window" 
:name  "robot-display-window" 
:save-bits  t 
:expose-p  t) ) 
(let*  ( (r-w  (send  *robot-display-window*  :width) ) 
(r-h  (send  *robot-display-window*  :height) ) 
(r-x  nil)   (r-y  nil) ) 
(multiple-value  (r-x  r-y)  (send  *robot-display-window*  :position) ) 
(setq  *robot-window*  (tv:make-window  ' 

tv: window 

:position  (list  r-x  r-y) 
: width  r-w 
: height  r-h 
:blinker-p  nil 
:borders  2 

: label  "robot-window" 
:name  "robot-window" 
:save-bits  t 
:expose-p  nil) ) 
(setq  *terrain-buf fer*  (tv:make-window 

' tv: window 

:position  (list  r-x  r-y) 
: width  r-w 
:height  r-h 
:blinker-p  nil 
: borders  2 

: label  "terrain-buffer" 
:name  "terrain-buffer" 
:save-bits  t 
:expose-p  nil) ) 
(setq  *max-y*  (send  *robot-window*  : inside-height) ) ) 
(setq  *robot-display-window-array*  (send  *robot-display-window*  :bit-array) 
(setq  *robot-window-array*  (send  *robot-window*  :bit-array) ) 
(setq  *robot-window-width*  (send  *robot-window*  : inside-width) ) 
(setq  * robot -window-height*  (send  *robot-window*  : inside-height) ) 
(setq  *terrain-buf fer-array*  (send  *terrain-buf fer*  :bit-array) ) ) 


(defun  make-visible  () 

(send  *robot-display-window*  :bitblt 

tv:alu-seta  *robot-window-width*  *robot-window-height* 
*robot-window-array*  2  2  0  0)) 
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(defun  move-to  (a-point) 
;  global  variables  :  *start-point* 
;  This  function  just  changes  *start-point* . 
(setq  *start-point*  a-point) ) 


(defun  save-terrain-to-terrain-buf f er  () 

(tv: sheet -force-access  ( * terrain-buffer*) 
(send  *terrain-buf f er*  :bitblt 

tv:alu-seta  *robot-window-width*  *robot-window-height* 
*robot-window-array*  2  2  0  0) ) ) 


(defun  clear-robot-window  () 

(tv: sheet -force-access  (* robot -window*) 
(send  *robot-window*  : clear-window) ) ) 


(defun  middle-of -robot-window  () 

(list  (/  (send  *robot-window*  : inside-width)  2) 

(/  (send  *robot-window*  : inside-height)  2))) 


(defun  kill-robot-terrain-windows () 
(send  *robot-display-window*  :kill) 
(send  *robot-window*  :kill) 
(send  *terrain-buf fer*  :kill)) 
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; ;  -*-  Mode : Common-Lisp;  Base: 10  -*- 
******************************************************************* 

H-calculator  definition 
******************************************************************* 


(def flavor  H-calculator ( (sampling-time  0.1)  H 

old-H) 

0 

: init able-instance -variables) 

(defmethod  (H-calculator  :initti) 

0 
;  library  fucntion  :  ident 

(setf  H   (ident) ) 

(setf  (aref  H  0  3)  6.5) 

(setf  (aref  H  1  3)  19.5) 

(setf  (aref  H  2  3)  5.4) 

H) 

(defmethod  (H-calculator  :new-H) 

(body-trans-rate  body- rot ate -rate) 
(setf  H 

(orthogonalization 
(get-new-H 
H 

(get-del-H 
H 
(get-delta  body-trans-rate  body-rotate-rate  sampling-time) ) ) ) ) ) 


(defmethod  (H-calculator  :save) 
() 
(setf  old-H  H) ) 


(defmethod  (H-calculator  : restore) 
0 
(setf  H  old-H) ) 


."******************************************************!■ 

H-calculator .new-H 

;"******************************************************n 

( 

(defun  get-delta  (body-trans-rate  body-rotate-rate  sampling-time) 
(let*  ( (del-trans-x  (*  (first  body-trans-rate)  sampling-time) ) 
(del-trans-y  (*  (second  body-trans-rate)  sampling-time) ) 
(del-trans-z  (*  (third  body-trans-rate)  sampling-time)) 
(del-rotate-x  (*  (first  body-rotate-rate)  sampling- time) ) 
(del-rotate-y  (*  (second  body-rotate-rate)  sampling-time) ) 
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(del-rotate-z  (*  (third   body-rotate-rate)  sampling-time))) 
(list  (list  del-trans-x   del-trans-y   del-trans-z) 

(list  del-rotate-x  del-rotate-y  del-rotate-z) ) ) ) 


(defun  get- 

■del-H  (H  delt 

;  math  lib 

:  ident 

(let*  ((H-del 

(ident) ) 

(delta- 

-trans   ( 

(delta- 

-rotate  ( 

(setf 

aref 

H-del  0 

(setf 

aref 

H-del  1 

(setf 

aref 

H-del  2 

(setf 

aref 

H-del  0 

(setf 

aref 

H-del  1 

(setf 

aref 

H-del  2 

(setf 

aref 

H-del  0 

(setf 

aref 

H-del  1 

(setf 

aref 

H-del  2 

(setf 

aref 

H-del  0 

(setf 

aref 

H-del  1 

(setf 

aref 

H-del  2 

(setf 

aref 

H-del  3 

(matri> 

mult 

H  H-del) 

a-trans-rotate) 

;  initialze  identity  matirix 
first   delta-trans-rotate) ) 
second  delta-trans-rotate)  )  ) 
0)  0) 

(third  delta-rotate) ) 

(-  (second  delta-rotate) ) ) 

(-  (third  delta-rotate) ) ) 

0) 

(first  delta-rotate) ) 

(second  delta-rotate) ) 

(-  (first  delta-rotate))) 

0) 

(first   delta-trans) ) 

(second  delta-trans)  ) 

(third   delta-trans)  ) 


0) 

0) 

1) 

1) 

1) 

2) 

2) 

2) 

3) 

3) 

3) 

3)  0) 

)) 


(defun  get-new-H  (H  del-H) 
(matrixadd  H  del-H) ) 
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...  _*_  Mode : Common-Lisp;  Base: 10  -*- 


.**************************************************************** 


leg  flavor  definition 


.•A************************************************************** 


(defflavor  leg  (name  owner  plan-machine  control-machine 

executor  contact-sensor  tkm-calculator 
foothold-finder  exchanged-leg 
foothold  tkm  foothold-list  tkm-list  tkm-p 
reserved-foothold  reserved-tkm) 
0 
: initable-instance-variables 
: get table-instance- variables) 


(defmethod  (leg  :initti) 
(H) 
(setf  contact-sensor   (make-instance  'contact-sensor   :owner  self)) 
(setf  executor         (make-instance  'executor         :owner  self)) 
(setf  control-machine  (make-instance  'control-state-machine  rowner  self)) 
(setf  plan-machine     (make-instance  'plan-state-machine     :owner  self)) 
(setf  tkm-calculator   (make-instance  'tkm-calculator   :owner  self)) 
(setf  foothold-finder  (make-instance  'foothold-finder  : owner  self)) 
(setf  foothold  (send  executor  :initti  name  H) ) 
(send  contact-sensor  :initti  name) 
(send  control-machine  :initti  name) 
(send  plan-machine  :initti  name) 
(send  tkm-calculator  :initti  name) 
(send  foothold-finder  rinitti  name)) 


(defmethod  (leg  : contact-confirm) 
() 
(send  contact-sensor  :contact-p) ) 


(defmethod  (leg  : do-planned-motion) 
0 
(send  plan-machine  : change) 
(send  control-machine  : change) 

(send  executor  :move  (send  owner  :get-Hl)  (send  owner  :get-inv-Hl) 
(send  owner  :get-body-trans-ratel) 
(send  owner  :get-body-rotate-ratel) ) 
(send  contact-sensor  : sensing)) 


(defmethod  (leg  :get-Hl) 
0 
(send  owner  :get-Hl)) 


(defmethod  (leg  :has-foothold-p) 
0 
foothold) 
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(defrnethod    (leg    :  interlock-conf  irrn) 

0 
;    may   add   stable-without-p   self 

(if    (send  exchanged-leg    :contact-conf irm) 
t 
nil)  ) 


(defrnethod    (leg    : leg-pos-wrt-body) 

0 

(send  executor  : leg-pos-wrt-body) ) 


(defrnethod  (leg  : lift-able) 
0 
(if  (equal  (send  plan-machine  : state-name)  ' eligible-to-lif t) 
self 
nil)  ) 


(defrnethod  (leg  :lift-ok) 
0 
(send  owner  :lift-ok  name)) 


(defrnethod  (leg  : lifted) 
0 
(send  owner  : lifted  name)) 


(defrnethod  (leg  : new-foothold) 
0 
(cond  ( (car  foothold-list) 
(send  self  :set-max) 
t) 
(t 
nil))) 


(defrnethod  (leg  :permitted-cell) 
(t-cell) 
(send  owner  :permitted-cell  t-cell) ) 


(defrnethod  (leg  : place-able) 

0 
;  check  plan  state  as  well, as  foothold  for  the  leg 

(if  (equal  (send  plan-machine  : state-name)  'available-leg) 
self   ' 
nil)) 


(defrnethod  (leg  :pro jected-pos) 
0 
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(send  executor  : get-desired-pos) ) 


(defmethod  (leg  : select-foothold) 

0 
;  out-list:  ( (max-f oothold  max-tkm)  (foothold-list)  (tkm-list)) 
(let*  ( (H  (send  owner  :get-H6)) 

(inv-H  (send  owner  :get-inv-H6) ) 

(body-trans-rate  (send  owner  :get-body-trans-ratelO) ) 
(body-rotate-rate  (send  owner  :get-body-rotate-ratelO) ) 
(estimated- support -plane 

(send  owner  :get-estimated-support-plane) ) 
(out-list 

(send  foothold-finder  : find-foothold 

H  inv-H  body-trans-rate  body-rotate-rate 
estimated-support-plane) ) ) 
(setf  foothold  (first  (first  out-list) ) ) 
(setf  reserved-foothold  foothold) 
(setf  tkm  (second  (first  out-list)  )  ) 
(setf  reserved-tkm  tkm) 
(setf  foothold-list  (second  out-list) ) 
(setf  tkm-list  (third  out-list)))) 


(defmethod  (leg  :send-decision) 
(a-decision) 
(send  plan-machine  : send-decision  a-decision) 
;   ) 


/(defmethod  (leg  : send-decision  :after) 
;  (a-decision) 

(if  (equal  a-decision  'place) 

(send  executor  :set-desired-pos  foothold))) 


(defmethod  (leg  :send-exchange) 
(a-leg) 
(setf  exchanged-leg  a-leg) ) 


(defmethod  (leg  : set-max) 
() 
(do  ( (footholds  (cdr  foothold-list)  (cdr  footholds) ) 
(tkms  (cdr  tkm-list)  (cdr  tkms) ) 
(max-foothold  (car  foothold-list) ) 
(max-tkm  (car  tkm-list)  ; 
(out-footholds)  (out-tkms) ) 
( (null  footholds) 
(setf  foothold  max-foothold) 
(setf  tkm  max-tkm)  , 
(setf  foothold-list  out-footholds) 
(setf " tkm-list  out-tkms)) 
(cond  ( (>  (car  tkms)  max-tkm) 

(setf  max-foothold  (car  footholds) ) 
(setf  max-tkm  (car  tkms))) 
(t 
(setf  out-footholds 

(cons  (car  footholds)  out-footholds) ) 
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(setf  out-tkms 

(cons  (car  tkms)  out-tkms)))))) 


(defmethod  (leg  : stable-without-p) 
0 
(send  owner  : stable-without-p  self)) 


(defmethod  (leg  : supporting) 
0 
(cond  ((equal  (send  plan-machine  :state-name)  'planned-contact) 
self) 

((equal  (send  plan-machine  :state-name)  ' eligible-to-lif t) 
self) 
(t  nil) ) 
) 


(defmethod  (leg  : support ing-p) 
0 
(cond  ((equal  (send  control-machine  :state-name)  'contact) 
self) 

((equal  (send  control-machine  :state-name)  'support) 
self) 
(t  nil)) 
) 


(defmethod  (leg  : terrain-point ) 
(t-cell) 
(send  owner  : terrain-point  t-cell) 


(defmethod  (leg  :TKM-limit) 
0 
(cond  ( (null  tkm) 
self) 

(  (<  tkm  0.1) 
self) 
(t 
nil))) 


(defmethod  (leg  :TKM-limit-p) 
0 
(cond  ( (null  tkm-p) 

self)  , 

( (<  tkm-p  0.5) 
self) 
(t  nil)) ) 


(defmethod  (leg  :update-tkm) 
0 
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(let  ((body-trans-rate  (send  owner  :get-body-trans-ratelO ) ) 

(body-rotate-rate  (send  owner  :get-body-rotate-ratelO ) ) 
(inv-H  (send  owner  :get-inv-H10) ) ) 
(setf  tkm  (send  tkm-calculator  :find-tkm 

(to-body-transf orm  inv-H  foothold) 
body-trans-rate  body-rotate-rate) ) ) 
) 


(defmethod  (leg  :update-tkm-p) 
0 
(let  ( (body-trans-rate-p  (send  owner  :get-body-trans-ratel) ) 

(body-rotate-rate-p  (send  owner  :get-body-rotate-ratel) ) 
(inv-H-p  (send  owner  :get-inv-Hl) ) ) 
(setf  tkm-p  (send  tkm-calculator  :find-tkm 

(to-body-transf orm  inv-H-p  foothold) 
body-trans-rate-p  body-rotate-rate-p) ) ) 
) 


(defmethod  (leg  : with-f oothold) 
0 
(cond  (reserved-foothold 

(setf  foothold  reserved-foothold) 
(setf  tkm  reserved-tkm) 
self) 
(t  nil))) 


math-t.lisp         Thu  Nov  29  11:32:16  1990         3 

;;;  -*-  Mode : Common-Lisp;  Package :USER;  Base: 10  -*- 


******************************************************************* 


robot  math  library 


******************************************************************* 


(defun  arc-cos  (s) 
(acos  s) ) 


(defun  col-mul (mat  coll  col2) 
(let  (  (sum  0)  ) 
(dotimes  (i  4) 

(setf  sum  (+  sum  (*  (aref  mat  i  coll)  (aref  mat  i  col2) ) ) ) 
sum)  ) 


(defun  counting (a-list) 

(do  (  (a-list  a-list  (cdr  a-list) ) 
(i  0  (+  i  1))) 
( (null  a-list)  i)  )  ) 


(defun  crossprod  (vectl  vect2) 

(let*  ( (xl  (first  vectl))  (x2  (first   vect2) ) 

(yl  (second  vectl))  (y2  (second  vect2)) 

(zl  (third  vectl))  (z2  (third  vect2) ) 

(x  (-  (*  yl  z2)  (*  y2  zl)  )  ) 

(y  (-  (*  x2  zl)  (*  xl  z2)  )  ) 

(z  (-  (*  xl  y2)  (*  x2  yl)  )  )  ) 
(list  x  y  z) ) ) 


(defun  delete-list  (a-list  b-list)    ;  delete  a-list  from  b-list 
(do   ( (deleting-li3t  a-list  (cdr  deleting-list) ) 
(deleted-list  b-list) ) 
( (null  deleting-list)  deleted-list) 
(setf  deleted-list  (remove  (car  deleting-list) 

deleted-list  :test  'equal)))) 


(def macro  dequeue  (queue) 
*  (progl  (car  , queue) 

(setf  , queue  (cdr  ,queue)))) 


(defun  dotprod  (vectl  vect2) 
;  No  dimension  limitation  ! ! ! 

(apply  '+  (mapcar  '*  vectl  vect2) ) ) 


(def macro  enqueue  (queue-name  element) 
;  globals  :  queue-name 
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;  Value  of  recover  field  of  command  is  a  list. 

;  Two  recover  command  is  possible  for  one  sampling-time. 

;  structure  of  QUEUE  :   (first  second  third  . . .  last) 

' (setq  , queue-name  (nconc  , queue-name  (list  , element))) 


(defmacro  empty-queue  (queue) 
'  (setq  , queue  '  ( ) ) ) 


(defun  ident () 

(make-array  '(4  4) : initial-contents 
'  (  (1  0  0  0) 
(0  10  0) 
(0  0  10) 
(0  0  0  1)) )  ) 


(defun  magnitude  (a-vector) 

(sqrt  (dotprod  a-vector  a-vector) 


(defun  magvect  (const  vect) 
;  magvect  =  const  *  vect 

(mapcar  #' (lambda  (a-element) 
(*  const  a-element)) 
vect) ) 


(defun  matrixadd  (mtl  mt2) 
(let  ( (mt3  (ident))) 
(dotimes  (i  4) 
(dotimes  (j  4) 

(setf  (aref  nit 3  i  j)  (+  (aref  mtl  i  j)  (aref  mt2  i  j))))) 
mt3)) 


(defun  matrixinv (mat) 

(let  ( (px   (-  (col-mul  mat  0  3))) 

(py   (-  (col-mul  mat  13))) 

(pz   (-  (col-mul  mat  2  3))) 

(matrix  (transpose  mat))) 
(setf  (aref  matrix  3  0)  0)   (setf  (aref  matrix  3  1)  0) 
(setf  (aref  matrix  3  2)  0)   (setf  (aref  matrix  3  3)  1) 
(setf  (aref  matrix  0  3)  px)  (setf  (aref  matrix  1  3)  py) 
(setf  (aref  matrix  2  3)  pz) 
matrix) ) 


(defun  matrixniult  (mtl  mt2) 

(let  ((mat  (make-array  '(4  4))))  ;it  defines  0  through  3.  (4  is  not  included) 
(dotimes  (i  4)       ;  will  repeat  i=0,  1,  2,  and  3.  (not  4) 
(dotimes  (j  4) 

(setf  (aref  mat  i  j)  0)  ;  initialize  to  zero 

(dotimes  (k  4) 

(setf  (aref  mat  i  j)  (+  (aref  mat  i  j)  (*  (aref  mtl  i  k) 
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(aref  mt2  k  j) )  )  )  )  )  ) 


mat)  ) 


(defun  nil-list (a-list) 

(do  ( (a-list  a-list  (cdr  a-list) ) 
(not-nil  nil) ) 
((null  a-list)  (not  not-nil)) 
(if  (car  a-list) 

(setf  not-nil  t) ) ) ) 


(defun  normalize-vector  (a-vector) 
(let  ( (m  (magnitude  a-vector) ) ) 
(if  (<  m  0.0000001) 
(list  0  0  0) 
(magvect  (/  1.0  m)  a-vector)))) 


(defun  orthogonalization  (mt) 

;  Gram-Schimit  orthogonalization  process 


(let*  ( (mx 
(tx 

(xl 

(yi 

(zl 
(ml 
(xl 

(yi 

(zl 

(a 

(x2 

<y2 

(z2 
(m2 
<x2 

<y2 

(z2 


ident) ) 

aref  mt  0  3))  (ty  (aref  mt  1  3))  (tz  (aref  mt  2  3) ) 


aref  mt  0  0))  (x2  (aref  mt  0  1))  (x3  (aref  mt 

aref  mt  1  0))  (y2  (aref  mt  1  1))  (y3  (aref  mt 

aref  mt  2  0) )  (z2  (aref  mt  2  1))  (z3  (aref  mt 

magnitude  (list  xl  yl  zl) ) ) 

/  xl  ml) ) 

/  yl  ml)) 

/  zl  ml) ) 

dotprod  (list  xl  yl  zl)  (list  x2  y2  z2) ) ) 

-  x2  (*  a  xl)  )  ) 

-  y2  (*  a  yl))) 

-  z2  (*  a  zl) )  ) 
magnitude  (list  x2  y2  z2))) 
/  x2  m2) ) 

/  y2  m2)) 
/  z2  m2))) 


2)) 
2)) 
2)) 


(setf 
(setf 
(setf 
(setf 
mx)  ) 


(aref  mx  0  0)  xl)  (setf  (aref  mx  0  1)  x2)  (setf  (aref  mx  0  2)  x3) 

(aref  mx  1  0)  yl)  (setf  (aref  mx  1  1)  y2)  (setf  (aref  mx  1  2)  y3) 

(aref  mx  2  0)  zl)  (setf  (aref  mx  2  1)  z2)  (setf  (aref  mx  2  2)  z3) 

(aref  mx  0  3)  tx)  (setf  (aref  mx  1  3)  ty)  (setf  (aref  mx  2  3)  tz) 


(defun  plane-transform  (  plane  matrix  ) 
;  Transf ormed-Plane  =  Plane  *  Matrix 


plane 
(let* 


( (a  b  c)  d) .  (a  b  c)  is  unit  normal,  d  is  -(distance) 


is  defined  as 
( (new-a  nil) 
(new-b  nil) 
(new-c  nil)       , 
(new-d  nil) 

(old-unit-normal  (car  plane) ) 
(old-d  (cadr  plane) ) 
(old-a  (first   old-unit-normal) ) 
(old-b  (second  old-unit-normal) ) 
(old-c  (third   old-unit-normal) ) 
(mag  nil) ) 
(setf  new-a  (+  (*  old-a  (aref  matrix  0 


0))  (*  old-b  (aref  matrix  1  0)) 
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(*  old-c  (aref  matrix  2  0))) 

(setf  new-b  (+  (*  old-a  (aref  matrix  0  1) ) 

(*  old-c  (aref  matrix  2  1))) 

(setf  new-c  (+  (*  old-a  (aref  matrix  0  2)) 

(*  old-c  (aref  matrix  2  2))) 

(setf  new-d  (+  (*  old-a  (aref  matrix  0  3) ) 

(*  old-c  (aref  matrix  2  3))  old-d) ) 

(setf  mag  (magnitude  (list  new-a  new-b  new-c) ) ) 
(if  (<  (abs  mag)  0.0000001) 

(print  "Error  in  PlaneTransf orm") 

(list  (list  (/  new-a  mag)  (/  new-b  mag)  (/  new-c  mag)) 
(/  new-d  mag) ) ) ) ) 


old-b  (aref  matrix  1  1) ) 
old-b  (aref  matrix  1  2)) 
old-b  (aref  matrix  1  3) ) 


(defun  plane-distance  (plane  velocity  position) 

;  Plane  (X  -  Q)N  =  0  ,   straight  line  X  =  P  +  tA. 

;t=  (Q-P)N/  (  AN  )   if  A  is  normalized  then  t  is  signed  distance 

;  if  t  is  infinitive  then  plane-distance   returnes  nil. 

;  plane-distance  returns  t. 

(let*  ( (A  (normalize-vector  velocity) ) 
(N  (first  plane) ) 
(dis  (-  (second  plane) ) ) 

(Q    (magvect  dis  N) )  ;  magvect  =  const  *  vector 
(P    position) 
(Q_P  (vectsub  Q  P) ) 
(AN   (dotprod  A  N) ) 
(numerator  (dotprod  Q_P  N) ) ) 
(if  (<  (abs  AN)  0.0000001)   ;  no  crossing 
nil  ;  returns  nil 

(/  numerator  AN)))) 


(defun  plane-intersection  (a-line  a-plane) 
;  a-line   ((direction)  (point))   X  =  P  +  tA. 
;  a-plane  ( (unit -normal)  -dist)   (X  -  Q)N  =  0. 

(let*  ( (velocity  (normalize-vector  (first  a-line) ) ) 
(position  (second  a-line) ) 

(t-value  (plane-distance  a-plane  velocity  position) ) ) 
(if  t-value 

(vectadd  position  (magvect  t-value  velocity) ) 
nil) ) )  ;  no  intersection 


(defun  plane-normal-distance  (a-plane  a-point) 
;  vector-type-plane  (abed) 
;  paul-type-point    transpose (x  y  z  1) 
(let*  ((unit-normal  (first   a-plane)) 
(dis         (second  a-plane) ) 

(vector-type-plane  (reverse  (cons  dis  (reverse  unit-normal) )) ) 
(paul-type-point    (reverse  (cons  1    (reverse  a-point))))) 
(dotprod"  vector-type-plane  paul-type-point) ) ) 


(defun  rotatemat (axis  angle)  ;  array  index  starts  from  0  not  1. 
;  return  rotatematrix    angle  : radian    axis  :  x  y  or  z 
(let  ( (mat  (ident) ) 
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(cosa  (cos 

angle) ) 

(sina  (sin 

angle) ) ) 

(case  axis 

(x-axis 

(setf 

(aref  mat 

1 

1) 

cosa) 

(setf 

(aref  mat 

2 

1) 

sina) 

(y-axis 

(setf 

(aref  mat 

0 

0) 

cosa) 

(setf 

(aref  mat 

2 

0) 

(-  sina) ) 

(z-axis 

(setf  (aref  mat  1  2)  (-  sina) ) 

(setf  (aref  mat  2  2)  cosa)) 

(setf  (aref  mat  0  2)  sina) 

(setf  (aref  mat  2  2)  cosa)) 

(setf  (aref  mat  0  0)   cosa)      (setf  (aref  mat  0  1)  (-  sina)) 

(setf  (aref  mat  1  0)   sina)   (setf  (aref  mat  1  1)  cosa))) 
mat))   ;  returns  this  value. 


(defun  to-body-transf orm  (inv-H  points-wrt-earth) 
;  returns  points-wrt-body 

(if  (listp  (first  points-wrt-earth) )  ;  test  multi-points 

(do  ( (points  points-wrt-earth  (cdr  points) )  ;  multi-points  case 
(out -points  nil) ) 
(  (null  points)  (reverse  out-points) ) 
(setf  out-points  (cons  (transform  inv-H  (car  points) )  out-points) ) ) 
(transform  inv-H  points-wrt-earth) ) )         ;  single  point  case 

(defun  to-earth-transf orm  (H  points-wrt-body) 
;  returns  points-wrt-earth 

(if  (listp  (first  points-wrt-body) )  ;  test  multi-points 

(do  ( (points  points-wrt-body  (cdr  points) )   ;  multi-points  case 
(out -points  nil) ) 
( (null  points)  (reverse  out-points) ) 
(setf  out-points  (cons  (transform  H  (car  points) )  out-points) ) ) 
(transform  H  points-wrt-body) ) )  ;  single  point  case 


(defun  transf orm(mat  point)   ;  array  index  starts  from  0  not  1. 
(let  ( (x  (car  point) ) 
(y  (cadr  point) ) 

(z  (if  (caddr  point)  (caddr  point)  0))) 
(list  (+  (*  x  (aref  mat  0  0))  (*  y  (aref  mat  0  1))  (*  z  (aref  mat  0  2)) 
(aref  mat  0  3) ) 
(+  (*  x  (aref  mat  10))  (*  y  (aref  mat  11))  (*  z  (aref  mat  1  2)) 

(aref  mat  1  3) ) 
(+  (*  x  (aref  mat  2  0))  (*  y  (aref  mat  2  1))  (*  z  (aref  mat  2  2)) 
(aref  mat  2  3) ) ) ) ) 


(defun  transmat  (x  y  z) 
;  returns  translational  marix 
(let  ((matrix  (ident))) 

(setf  (aref  matrix  0  3,)  x) 
(setf  (aref  matrix  1  3)  y) 
(setf  (aref  matrix  2  3)  z) 
matrix) ) 


(defun  transpose  (mat) 

(let  ((matrix  (make-array  '  (4  4) 
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(dotimes  (i  4) 
(dotimes  (j  4) 

(setf  (aref  matrix  i  j)  (aref  mat  j  i) ) ) ) 
matrix) ) 


(defun  unit-crossprod  (vectl  vect2) 
;  generate  unitnormal  vector  of  vectl  X  vect2 
(let*  ( (xl  (first   vectl))  (x2  (first   vect2)) 

(yl  (second  vectl))  (y2  (second  vect2)) 

(zl  (third   vectl))  (z2  (third   vect2) ) 

(x  (-  (*  yl  z2)  (*  y2  zl) ) ) 

(y  (-  (*  x2  zl)  (*  xl  z2))) 

(z  (-  (*  xl  y2)  (*  x2  yl))) 

(m  (sqrt  (+  (*  x  x)   (*  y  y)  (*  z  z)  )  )  )  ) 
(list  (/  x  m)  (/  y  m)  (/  z  m)  )  )  ) 


(defun  vectadd  (vectl  vect2) 
;  vectsub  =  vectl  +  vect2 
;  no  limit  in  dimension 
(mapcar  '+  vectl  vect2) ) 


(defun  vectsub  (vectl  vect2) 
;  vectsub  =  vectl  -  vect2 
;  no  limit  in  dimension 
(mapcar  '-  vectl  vect2)) 
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; ;  -*-  Mode : Common-Lisp;  Base: 10  -*- 
******************************************************************* 

over lap- foot ho Id- finder  definition 
******************************************************************* 

(def flavor  overlap-foothold-finder (adjacent-leg-numbers) 
(foot ho Id- finder) 
: init able- instance -variables) 


(defmethod  (overlap-foothold-finder  :initti) 
(leg-name) 
(cond  ((equal  leg-name  'legl) 

(setf  adjacent-leg-numbers  ' (3) ) 
(setf  sixteen-f ootholds 

'((  9.0   4.3)  (  9.0   3.3)  (  9.0   2.3)  (  9.0   1.3) 

(  8.0   4.3)  (  8.0   3.3)  (  8.0   2.3)  (  8.0   1.3) 

(  7.0   4.3)  (  7.0   3.3)  (  7.0   2.3)  (  7.0   1.3) 

(  6.0   4.3)  (  6.0   3.3)  (  6.0   2.3)  (  6.0   1.3) 

(  5.0   4.3)  (  5.0   3.3)  (  5.0   2.3)  (  5.0   1.3) 

(  4.0   4.3)  (  4.0   3.3)  (  4.0   2.3)  (  4.0   1.3) 

(  3.0   4.3)  (  3.0   3.3)  (  3.0   2.3)  (  3.0   1.3))) 
(setf  four-lines 

'(((0   0.3420  -0.9397)  (   9.5   2.7339  0)) 

((0  -0.3420  -0.9397)  (   9.5   2.7339  0)) 

((0  -0.3420  -0.9397)  (   2.5   2.7339  0)) 

((0   0.3420  -0.9397)  (   2.5   2.7339  0))))) 
((equal  leg-name  'leg2) 
(setf  adjacent-leg-numbers  '  (4)  ) 
(setf  sixteen-f ootholds 

'((  9.0  -4.3)  (  9.0  -3.3)  (  9.0  -2.3)  (  9.0  -1.3) 

(  8.0  -4.3)  (  8.0  -3.3)  (  8.0  -2.3)  (  8.0  -1.3) 

(  7.0  -4.3)  (  7.0  -3.3)  (  7.0  -2.3)  (  7.0  -1.3) 

(  6.0  -4.3)  (  6.0  -3.3)  (  6.0  -2.3)  (  6.0  -1.3) 

(  5.0  -4.3)  (  5.0  -3.3)  (  5.0  -2.3)  (  5.0  -1.3) 

(  4.0  -4.3)  (  4.0  -3.3)  (  4.0  -2.3)  (  4.0  -1.3) 

(  3.0  -4.3)  (  3.0  -3.3)  (  3.0  -2.3)  (  3.0  -1.3))) 
(setf  four-lines 

'(((0   0.3420  -0.9397)  (   9.5  -2.7339  0)) 

((0  -0.3420  -0.9397)  (   9.5  -2.7339  0)) 

((0  -0.3420  -0.9397)  (   2.5  -2.7339  0)) 

((0   0.3420  -0.9397)  (   2.5  -2.7339  0))))) 
((equal  leg-name  'leg3) 
(setf  adjacent-leg-numbers  ' (1  5)) 
(setf  sixteen-footholds 

'((  3.0   4.3)  (  3.0   3.3)  (  3.0   2.3)  (  3.0   1.3) 

(  2.0   4.3)  (  2.0   3.3)  (  2.0   2.3)  (  2.0   1.3) 

(  1.0   4.3)  (  1.0   3.3)  (  1.0   2.3)  (  1.0   1.3) 

(  0.0   4.3)  (  0.0   3.3)  (  0.0   2.3)  (  0.0   1.3) 

(-1.0   4.3)  (-1.0   3.3)  (-1.0   2.3)  (-1.0   1.3) 

(-2.0   4.3)  (-2.0   3.3)  (-2.0   2.3)  (-2.0   1.3) 

(-3.0   4.3)  (-3.0   3.3)  (-3.0   2.3)  (-3.0   1.3))) 
(setf  four-lines  , 

'(((0   0.3420  -0.9397)  (  3.5   2.7339  0)) 
((0  -0.3420  -0.9397)  (  3.5   2.7339  0)) 
((0  -0.3420  -0.9397)  (-3.5   2.7339  0)) 
((0   0.3420  -0.9397)  (-3.5   2.7339  0))))) 
((equal  leg-name  'leg4) 
(setf  adjacent -leg-numbers  '  (2  6)  ) 
(setf  sixteen-footholds 

'((  3.0   -4.3)  (  3.0   -3.3)  (  3.0   -2.3)  (  3.0   -1.3) 
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(  2 
(  1 
(  0 
(-1 
(-2 
(-3 


-4.3) 
-4.3) 
-4.3) 
-4.3) 
-4.3) 
-4.3) 


(  2 
(  1 


(  0.0 


(-1 
(-2 
(-3 


-3.3) 
-3.3) 
-3.3) 
-3.3) 
-3.3) 
-3.3) 


(  2 

(  1 
(  0 
(-1 
(-2 
(-3 


-2.3) 
-2.3) 
-2.3) 
-2.3) 
-2.3) 
-2.3) 


{  2.0 
(  1.0 
(  0.0 
(-1.0 
(-2.0 
(-3.0 


(setf  four-lines 

'  (  (  (0   0.3420  -0.939 

(  (0  -0.3420  -0.939 

(  (0  -0.3420  -0.939 

(  (0   0.3420  -0.939 

((equal  leg-name  'leg5) 

(setf  adjacent-leg-numbers 

(setf  sixteen-f ootholds 


7) 
7) 
7) 

7) 


(  3.5  -2 

(  3.5  -2 

(-3.5  -2 

(-3.5  -2 


7339  0) ) 
7339  0) ) 
7339  0) ) 
7339  0) ))) ) 


((-3 
(-4 
(-5 
(-6 
(-7 
(-8 
(-9 


3) 
3) 
3) 
3) 
3) 
3) 
3) 


(-3 

(-4 

(-5 

(-6, 

(-7 

(-8 

(-9 


(setf  four-lines 

'  (  (  (0   0.3420  -0.939 

(  (0  -0.3420  -0.939 

( (0  -0.3420  -0.939 

(  (0   0.3420  -0.939 

((equal  leg-name  'leg6) 

(setf  adjacent-leg-numbers 

(setf  sixteen-f ootholds 

' ( (-3.0  -4.3)  (-3.0 

(-4.0  -4.3)  (-4.0 

(-5.0  -4.3)  (-5.0 

(-6.0  -4.3)  (-6.0 

(-7.0  -4.3)  (-7.0 

(-8.0  -4.3)  (-8.0 

(-9.0  -4.3)  (-9.0 

(setf  four-lines 

'  ( ( (0   0.3420  -0.939 

( (0  -0.3420  -0.939 

( (0  -0.3420  -0.939 

( (0   0.3420  -0.939 


(3)) 

.3) 
.3) 
.3) 
.3) 
.3) 
.3) 
.3) 


(-3 
(-4 
(-5 
(-6 
(-7 
(-8 
(-9 


3) 
3) 
3) 
3) 
3) 
3) 
3) 


(-3 

(-4 
(-5 
(-6 
(-7 
(-8 
(-9 


(-2.5  2 

(-2.5  2 

(-9.5  2 

(-9.5  2 


-3 

-3 
-3 
-3 
-3 

-3 
-3 

7) 
7) 
7) 
7) 


(4) 

3) 
3) 
3) 
3) 
3) 
3) 
3) 


(-3.0 
(-4.0 
(-5.0 
(-6.0 
(-7.0 
(-8.0 
(-9.0 


(-2.5  -2 

(-2.5  -2 

(-9.5  -2 

(-9.5  -2 


7339  0)) 

7339  0)) 

7339  0)) 

7339  0))))) 


-1.3) 
-1.3) 
-1.3) 
-1.3) 
-1.3) 
-1.3) )) 


3) 
3) 
3) 
3) 
3) 
3) 
3))) 


7339  0) ) 
7339  0) ) 
7339  0) ) 
7339  0) ) ) ) ) 


-2.3)  (-3.0  -1.3) 

-2.3)  (-4.0  -1.3) 

-2.3)  (-5.0  -1.3) 

-2.3)  (-6.0  -1.3) 

-2.3)  (-7.0  -1.3) 

-2.3)  (-8.0  -1.3) 

-2.3)  (-9.0  -1.3) )) 


) 
(setf  tkm-calculator  (send  owner  : tkm-calculator) ) 
) 


(defmethod  (overlap-foothold-finder  :get-possible-f ootholds) 

(estimated-foothold3  H  inv-H) 
;  returns  possible-footholds  wrt  body 
;  find-possible-footholds  function  tests  obstacles 
(to-body-transform 

inv-H  , 

(send  self  :get-rid-of-overlap 

(send  self  : find-possible-footholds 

(to-earth-transf orm  H  estimated-f ootholds) )) ) 


(defmethod  (overlap-foothold-finder  :get-rid-of-overlap) 
(foot ho Ids -wrt -earth) 
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(let*  (  (a jacent-legs 
(mapcar 

#' (lambda  (leg-num) 

(■send  owner  : nth-leg  leg-num)) 
adjacent-leg-numbers) ) 
(ad jacent-legs -in-possible -inter act ion 
(remove 
nil 
(mapcar  #' (lambda  (leg) 

(if  (send  leg  :place-able)  ;no  interaction 
nil 
leg)) 
a jacent-legs) ) ) ) 
(send  self  : remove-overlapped-f oothold 
f oot holds -wrt -earth 
adjacent-legs-in-possible-interaction) ) ) 


(defmethod  (overlap-foothold-finder  : remove-overlapped-f oothold 
(foot holds -wrt -earth  legs -in-possible- inter act ion) 
(do  ( (legs  legs-in-possible-interaction  (cdr  legs) ) 
(out -foot ho Ids  foot ho Ids -wrt -earth) 
(overlap-foothold) ) 
(  (null  legs)  out-footholds) 
(setf  overlap-foothold  (send  (car  legs)  : foothold)) 
(setf  out-footholds 

(remove  overlap-foothold 
out -foot ho Ids 
:test  #' (lambda  (xl  x2) 

(send  self  :overlap-p  xl  x2) ) 
)))) 


(defmethod  (overlap-foothold-finder  :overlap-p) 
(xl  x2) 
(let  ( (xl-integer  (mapcar  #' truncate  xl) ) 
(x2-integer  (mapcar  #'truncate  x2) ) ) 
(equal  xl-integer  x2-integer) ) ) 


overlap-leg-t . lisp 


Thu  Nov  29  11:32:57  1990 


; ;  -*-  Mode : Common-Lisp;  Base: 10  -*- 
******************************************************************* 


overlap-leg  definition 


******************************************************************* 


(defflavor  overlap-leg  () 
(leg) 
) 


(defmethod  (overlap-leg  rinitti) 
(H) 
(setf  contact-sensor   (make-instance  'contact-sensor   :owner  self)) 
(setf  executor         (make-instance  'executor         :owner  self)) 
(setf  control-machine  (make-instance  'control-state-machine  : owner  self)) 
(setf  plan-machine     (make-instance  'plan-state-machine     rowner  self)) 
(setf  tkm-calculator   (make-instance  ' overlap-tkm-calculator   :owner  self)) 
(setf  foothold-finder  (make-instance  'overlap-foothold-finder  : owner  self)) 
(setf  foothold  (send  executor  :initti  name  H) ) 
(send  contact-sensor  :initti  name) 
(send  control-machine  :initti  name) 
(send  plan-machine  rinitti  name) 
(send  tkm-calculator  rinitti  name) 
(send  foothold-finder  rinitti  name)) 


(defmethod  (overlap-leg  :nth-leg) 
(leg-num) 
(send  owner  : nth-leg  leg-num) ) 


(defmethod  (overlap-leg  : foothold) 


0 


foothold) 
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..  _*_  Mode : Common-Lisp;  Base: 10  -*- 
******************************************************************* 

overlap-robot  definition 
******************************************************************* 


(def flavor  overlap-robot ( ) 
(robot) 
:init able- instance -variables 
: get table- instance- variables) 


:owner  self) ) 


(defmethod  (overlap-robot  :initti) 
0 
(send  graph-asv  :init-data) 

(setf  vision-system  (make-instance  'vision-system 
(send  vision-system  rinitti) 
(setf  joystick  (make-instance  'joystick)) 
(send  joystick  :reset) 
(empty-queue  lift-queue) 
(setf  lift-flag  t) 
(let  ((H)) 

(setf  body  (make-instance  'body  :owner  self)) 
(setf  H  (send  body  :initti)) 
(setf  legs  (list 

(make-instance  'overlap-leg 
(make-instance  'overlap-leg 
(make-instance  'overlap-leg 
(make-instance  'overlap-leg 
(make-instance  'overlap-leg 
(make-instance  'overlap-leg 
)  ) 
(mapcar  #' (lambda  (a-leg)  (send  a-leg  :initti  H) )  legs)) 


name 

'legl 

: owner 

self) 

name 

'leg2 

:  owner 

self) 

name 

'leg3 

:  owner 

self) 

name 

'leg4 

:  owner 

self) 

name 

'leg5 

:  owner 

self) 

name 

'leg6 

: owner 

self) 

(defmethod  (overlap-robot  :nth-leg) 

(leg-num) 
;  nth  starts  counting  from  zero. 
;  leg-num  starts  from  one. 
(nth  (-  leg-num  1)  legs) ) 


overlap-tkm-t . lisp 


Thu  Nov  29  11:33:28  1990 


;;  -*-  Mode  : Common-Lisp;  Base:  10  -*- 
******************************************************************* 

overlap-tkm-calculator  definition 
******************************************************************* 


(def flavor  overlap-tkm-calculator () 
(tkm-calculator) 
: init able -instance -variables) 


(defmethod  (overlap-tkm-ca 

(leg-name) 

(cond  ( (equal  leg-name  ' 

(setf  working-vol 

'  (  (  ( (0  0  1) 

(((0  0  1) 

( (equal  leg-name  ' 

(setf  working-vol 

'  (  (((0  0  1) 

(((0  0  1) 

( (equal  leg-name  ' 

(setf  working-vol 

'  ((((0  0  1) 

(((0  0  1) 

( (equal  leg-name  ' 

(setf  working-vol 

' ((((0  0  1) 

(((0  0  1) 

( (equal  leg-name  ' 

(setf  working-vol 

'  ((((0  0  1) 

(((0  0  1) 

( (equal  leg-name  ' 

(setf  working-vol 

' ((((0  0  1) 

(((0  0  1) 

) 

) 


lculator  :initti) 


legl) 

ume 

3.316) 

(d 

0 

0) 

-9 

5) 

(0 

0 

9397 

0 

^420) 

-2 

569) 

5.7313) 

(  (1 

0 

0) 

-2 

5) 

(0 

0 

9397 

-0 

^420) 

-2 

569) 

leg2) 

ume 

3.316) 

((1 

0 

0) 

-9 

5) 

(0 

0 

9397 

0 

3420) 

2 

.569) 

5.7313) 

(  (1 

0 

0) 

-2 

5) 

(0 

0 

9397 

-0 

3420) 

2 

.569) 

leg3) 

ume 

3.316) 

(  (1 

0 

0) 

-3 

5) 

(0 

0 

9397 

0 

.3420) 

-2 

.569) 

5.7313) 

(  (1 

0 

0) 

3 

.5) 

(0 

0 

9397 

-0 

.3420) 

-2 

.569) 

leg4) 

ume 

3.316) 

(d 

0 

0) 

-3 

5) 

(0 

0 

9397 

0 

.3420) 

2 

.569) 

5.7313) 

(  (1 

0 

0) 

3 

5) 

(0 

0 

9397 

-0 

.3420) 

2 

.569) 

leg5) 

ume 

3.316) 

(  (1 

0 

0) 

2 

5) 

(0 

0 

9397 

0 

.3420) 

-2 

.569) 

5.7313) 

(  (1 

0 

0) 

9 

5) 

(0 

0 

9397 

-0 

.3420) 

-2 

.569) 

leg6) 

ume 

3.316) 

(d 

0 

0) 

2 

5) 

(0 

0 

9397 

0 

3420) 

2 

569) 

5.7313) 

(d 

0 

0) 

9 

5) 

(0 

0 

9397 

-0 

3420) 

2 

569) 

)) ) 


)) 


))) 


)) 


))) 


))) 
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; ; ;  -*-  Mode : Common-Lisp;  Base: 10  -*- 


******************************************************** 


plan-state  flavor  definition 


******************************************************** 


(def flavor  plan-state  ( (decision  nil)  (observation  nil)  (command  nil) 

(condition  nil) ) 
(state) 
: init able- instance -variables) 


(defmethod  (plan-state  :generate-command) 


() 


command) 


(defmethod  (plan-state  : change) 

(given-decision  observed-state  given-condition) 
(cond  ( (and  decision  (listp  decision)  ) 

(cond  ( (equal  given-decision  (first  decision) ) 
(first  next-state) ) 
( (equal  given-decision  (second  decision) ) 

(second  next-state) ) 
(t  self))) 
(condition 
(if  (and  (equal  given-cone ition  condition) 

(equal  observed-state  observation) ) 
next-state 
self) ) 
(t 
(cond  ( (equal  observed-state  observation) 
next-state) 

( (equal  given-decision  decision) 
next-state) 
(t  self))))) 


******************************************************** 


plan-state-machinie  flavor  definition 


******************************************************** 


(defflavor  plan-state-machine  ( (decision  nil)  (observation  nil) 

(condition  nil)  (lift-ready-flag  nil) 
,     control-machine) 
(state-machine) 
: initable-instance-variables) 


(defmethod  (plan-state-machine  rinitti) 
(leg-name) 
(if  (member  leg-name  ' (legl  leg4  leg5) ) 
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(send  self  : init-plan-machine  ' eligible-to-lif t ) 
(send  self  : init-plan-machine  'available-leg)) 
(setf  control-machine  (send  owner  : control-machine) ) ) 


(defmethod  (plan-state-machine  :  init-plan-machine) 
(a-state-name) 
(let  (available-leg  planned-contact  eligible-to-lif t 
planned-lift   actual-lift  planned-exchange) 
(setf  actual-lift 

(make-instance  'plan-state 

:name  'actual-lift 
: observation  'ready 
•.command  '  recover-command)  ) 
(setf  planned-lift 

(make-instance  'plan-state 

:name  'planned-lift  : condition  'stable-without 
: observation  'support 
:next-state  actual-lift)) 
(setf  planned-exchange 

(make-instance  'plan-state 

:name  'planned-exchange  rcondition  ' interlock-conf irm 
robservation  'support 
:next-state  actual-lift)) 
(setf  eligible-to-lift 

(make-instance  'plan-state 

:name  'eligible-to-lift 
:decision  ' (lift  exchange) 

:next-state  (list  planned-lift  planned-exchange) ) ) 
(setf  planned-contact 

(make-instance  'plan-state 

-.name  'planned-contact  : observation  'contact 
: command  'deploy-command 
:next-state  eligible-to-lift)) 
(setf  available-leg 

(make-instance  'plan-state 

:name  'available-leg  : decision  'place 
:next-state  planned-contact)) 
(send  actual-lift  : set-next-state  available-leg) 

(setf  state  (cond  ((equal  a-state-name  (send  available-leg  : state-name) ) 
available-leg) 

((equal  a-state-name  (send  planned-contact  : state-name) ) 
planned-contact ) 

((equal  a-state-name  (send  eligible-to-lift  : state-name) ) 
eligible-to-lift) 

((equal  a-state-name  (send  planned-lift  : state-name) ) 
planned-lift) 

((equal  a-state-name  (send  planned-exchange  : state-name) ) 
planned-exchange) 

((equal  a-state-name  (send  actual-lift  :state-name) ) 
actual-lift) ) ) 
) 
) 


(defmethod  (plan-state-machine  : change  rbefore) 
() 
(setf  observation  (send  control-machine  : state-name) ) 
(cond  ((and  (equal  (send  state  : state-name)  'planned-exchange) 
(send  owner  : interlock-conf irm) 
(send  owner  : stable-without -p) 
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(send  owner  :lift-ok)) 

(setf  lift-ready-flag  t) 

(setf  condition  '  interlock-conf irm) ) 
((and  (equal  (send  state  :state-narne) 
(send  owner  : stable-without-p) 
(send  owner  :lift-ok)) 

(setf  lift-ready-flag  t) 

(setf  condition  'stable-without)) 
(t 

(setf  condition  nil) 

(setf  lift-ready-flag  nil) ) ) 


' planned-lift) 


(defmethod  (plan-state-machine  rchange) 
0 
(setf  observation  (send  control-machine  : state-name) ) 
(cond  ((and  (equal  (send  state  :state-name)  'planned-exchange) 
(send  owner  : interlock-conf irm) 
(send  owner  : stable-without-p) 
(send  owner  :lift-ok)) 
(setf  lift-ready-flag  t) 
(setf  condition  ' interlock-conf irm)  ) 
((and  (equal  (send  state  :state-name)  'planned-lift) 
(send  owner  : stable-without-p) 
(send  owner  :lift-ok)) 
(setf  lift-ready-flag  t) 
(setf  condition  'stable-without)) 
(t 
(setf  condition  nil) 
(setf  lift-ready-flag  nil) ) ) 
(setf  state  (send  state  :change  decision  observation  condition) ) 


; (defmethod  (plan-state-machine  :change  :after) 
0 
(send  control-machine  : send-command 

(send  state  rgenerate-command) ) 
(if  (and  lift-ready-flag 

(equal  (send  self  : state-name)  'actual-lift)) 
(send  owner  : lifted))) 


(defmethod  (plan-state-machine 
(a-decision) 
(setf  decision  a-decision) ) 


send-decision) 
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; ; ;  -*-  Mode : Common-Lisp;  Base:  10  -*- 

******************************************************** 

robot  flavor  definition 
******************************************************** 

(def flavor  robot (legs  body  vision-system  joystick 
(lift-able-legs  nil) 

(place-able-legs  nil)  (supporting-legs  nil) 
(supporting-p-legs  nil) 

(joy-command  '(0  0  0))  lift-queue  lift-flag) 
0 
: initable-instance-variables 
: get table- instance- variables) 


(defmethod  (robot  :initti) 
0 
(send  graph-asv  :init-data) 

(setf  vision-system  (make-instance  ' vision-sy3tem  :owner  self)) 
(send  vision-system  :initti) 
(setf  joystick  (make-instance  'joystick)) 
(send  joystick  : reset) 
(empty-queue  lift-queue) 
(setf  lift-flag  t) 
(let  ((H)) 

(setf  body  (make-instance  'body  :owner  self)) 
(setf  H  (send  body  :initti)) 
(setf  legs  (list 

(make-instance  'leg  :name  ' legl  :owner  self) 
(make-instance  'leg  :name  ' leg2  : owner  self) 
(make-instance  'leg  :name  ' leg3  : owner  self) 
(make-instance  'leg  :name  ' leg4  : owner  self) 
(make-instance  'leg  :name  ' leg5  : owner  self) 
(make-instance  'leg  :name  ' leg6  : owner  self) 
)) 
(mapcar  #' (lambda  (a-leg)  (send  a-leg  :initti  H) )  legs)) 
) 


(defmethod  (robot  : find-lift-able-legs) 
0 
(delete  nil  (mapcar  #' (lambda  (a-leg)  (send  a-leg  : lift-able))  legs))) 


(defmethod  (robot  : find-place-able-legs) 
0 
(delete  nil  (mapcar  #' (lambda  (a-leg)  (send  a-leg  :place-able) )  legs) ) ) 


(defmethod  (robot  : find-supporting-legs) 
0 
(delete  nil  (mapcar  #' (lambda  (a-leg)  (send  a-leg  supporting))  legs))) 
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(defmethod  (robot  : f ind-supporting-p-legs) 
0 
(delete  nil  (mapcar  #' (lambda  (a-leg)  (send  a-leg  : supporting-p) )  legs))) 


(defmethod  (robot  :get-body-rotate-ratel) 
0 
(send  body  :get-body-rotate-ratel) ) 


(defmethod  (robot  :get-body-rotate-ratelO) 
0 
(send  body   :get-body-rotate-ratelO) ) 


(defmethod  (robot  :get-body-trans-ratel) 
() 
(send  body  :get-body-trans-ratel) ) 


(defmethod  (robot  :get-body-trans-ratelO; 
0 
(send  body  :get-body-trans-ratelO) ) 


(defmethod  (robot  :get-estimated-support-plane) 
0 
(send  body  :get-estimated-support-plane)  ) 


(defmethod  (robot  :get-Hl) 
0 
(send  body  :get-Hl) ) 


(defmethod  (robot  :get-H6) 
() 
(send  body  :get-H6) ) 


(defmethod  (robot  :get-H10) 
0 
(send  body  :get-H10) ) 


(defmethod  (robot  :get-inv-Hl) 
0  . 

(send  body  :get-inv-Hl) ) 


(defmethod  (robot  :get-inv-H6) 
0 
(send  body  :get-inv-H6) ) 
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(defmethod  (robot  :get-inv-H10) 
() 
(send  body  :get-inv-H10) ) 


(defmethod  (robot  :lift-ok) 
(leg-name) 
(cond  (lift-flag 

(cond  ( (equal  leg-name  (send  (first  lift-queue)  :name) ) 
(setf  lift-flag  nil) 
t) 
(t 

nil)  )  ) 
(t  nil))) 


(defmethod  (robot  : lifted) 
(leg-name) 
(if  (equal  leg-name  (send  (first  lift-queue)  :name) ) 
(dequeue  lift-queue) 
(print  (list  "error  in  lifting"  leg-name) ) ) ) 


(defmethod  (robot  :permitted-cell) 
(t-cell) 
(send  vision-system  :permitted-cell  t-cell) ) 


(defmethod  (robot  : scanning) 
0 
(send  vision-system  : scanning)) 


(defmethod  (robot  : stable-without -p) 
(a-leg) 
(send  body  :stable-p 

(remove  a-leg  support ing-p-legs) ) ) 


(defmethod  (robot  :terrain-point) 
(t-cell) 
(send  vision-system  : terrain-point  t-cell) 


********************************************************** 


prolog  interface  robot  methods 


*********************************************************** 
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(def method  (robot  :  at-tk.m-limit) 
0 
(let  ( (limit-leg 

(car  (delete  nil 

(mapcar  #'  (lambda  (a-leg)  (send  a-leg  :TKM-lirnit )  )  lift-able-legs))))) 
(setf  supporting-legs  (remove  limit-leg 

supporting-legs) ) 
(setf  lift-able-legs  (remove  limit-leg 

lift-able-legs) ) 
limit-leg) ) 


(defmethod  (robot  :check-stability-p) 
0 
(send  body  :stable-p-m  supporting-p-legs  (first  lift-queue)) 


(defmethod  (robot  :check-tkm-limit-p) 
0 
(delete  nil 

(mapcar  #' (lambda  (a-leg)  (send  a-leg  :TKM-limit-p) )  supporting-p-legs))) 


(defmethod  (robot  : do-recovery) 
() 
(car 

(delete  nil 

(mapcar  #' (lambda  (a-leg)  (send  a-leg  : with-f oothold) )  place-able-legs)) 


(defmethod  (robot  : execute-planned-motion) 
0 
(mapcar  #'  (lambda  (a-leg)  (send  a-leg  : do-planned-motion)  )  legs) ) 


(defmethod  (robot  : graphical-display) 
0 
(send  graph-asv  rdisplay  (send  body  :get-Hl) 

(mapcar  #' (lambda  (a-leg)  (send  a-leg  : leg-pos-wrt-body) )  legs)) 

) 


(defmethod  (robot  :has-more-tkm) 
(legl  leg2) 
(>  (send  legl  :tkm) 
(send  leg2  :tkm)  ) ) 


(defmethod  (robot  : leg-with-new-f oothold) 

(> 
;  return  a-leg  with  new-foothold. 

(do  ((new-foothold-flags  (mapcar  #' (lambda  (a-leg)  (send  a-leg  :new-foothold) )  plac 
e-legs) 

(mapcar  #' (lambda  (a-leg)  (send  a-leg  :new-f oothold) )  plac 
e-legs) ) 

(a-leg  nil)  ) 
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(  (or  (nil-list  new-foothold-flags) 
a-leg) 
(if  a-leg  a-leg  nil) ) 
(setf  a-leg  (send  self  :max-sm-leg  nil) ) ) ) 


(defmethod  (robot  :max-sm-leg) 

(a-leg) 
;  max-sm-leg  without  supporting  a-leg 
(let  (legs-with-f oothold) 
(cond  (place-able-legs 

(setf  legs-with-f oothold 

(remove  nil  (mapcar  #' (lambda  (leg) 

(if  (send  leg  :has-f oothold-p) 
leg 
nil)  ) 
place-able-legs) ) ) 
(cond  (legs-with-f oothold 

(do  ( (legs  (cdr  legs-with-foothold)  (cdr  legs) ) 

(largest-leg  (car  legs-with-foothold)  largest-leg) 
(temp-support-legs  (remove  a-leg  supporting-legs) ) ) 
( (null  legs) 
(if  (send  body  :stable  (cons  largest-leg  temp-support-legs)) 
largest-leg 
nil)  ) 
(if  (send  body  :more-stable  temp-support-legs 
(car  legs)  largest-leg) 
(setf  largest-leg   (car  legs) ) ) ) ) 
(t  nil) ) ) 
(t  nil)))) 


(defmethod  (robot  :modif y-command) 
() 
(send  body  :modif y-command)  ) 


(defmethod  (robot  : wait-f or-lif t) 
0 
(delete  nil 

(mapcar  #'  (lambda  (a-leg)  (send  a-leg  : lift-not-done) )  supporting-p-legs) ) ) 

(defmethod  (robot  : read- joystick) 
0 
(let  ((joy-value  (send  joystick  :get- joy-value) ) ) 
(setf  joy-command 

(reverse  (cdr  (reverse  (send  joystick  :get- joy-value) ) ) ) ) 
(if  (fourth  joy-value) 
nil 
t))) 


(defmethod  (xobot  : res tore-command) 
0 
(send  body  :restore-command) ) 


(defmethod  (robot  :before  : send-decision) 
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(legl  leg2  a-decision) 
(cond  ((equal  a-decision  'exchange) 
(enqueue  lift-queue  legl) ) 
((equal  a-decision  'lift) 
(enqueue  lift-queue  legl) ) ) ) 


(defmethod  (robot  :send-decision) 
(legl  leg2  a-decision) 
(cond  ((equal  a-decision  'exchange) 

(send  legl  : send-decision  a-decision) 
(send  leg2  : send-decision  'place) 
(send  legl  :send-exchange  leg2)) 
(t 
(send  legl  : send-decision  a-decision)))) 


(defmethod  (robot  : smallest-tkm-leg) 

0 
;  select  smallest-TKM-leg 
;  tkm  is  nil  or  positive 

(do  ( (legs  (cdr  lift-able-legs)  (cdr  legs) ) 
(smallest-leg   (car  lift-able-legs) ) 
(smallest -tkm  nil)  (tkm  nil) ) 
(  (null  legs)  smallest-leg) 
(setf  smallest-tkm  (if  (send  smallest-leg  :tkm) 

(send  smallest-leg  :tkm)  -1000)) 
(setf  tkm  (if  (send  (car  legs)  :tkm) 

(send  (car  legs)  :tkm)  -1000)) 
(if  (>  smallest-tkm  tkm)  (setf  smallest-leg  (car  legs) ) ) 
(if  (and  (equal  smallest-tkm  -1000)  (equal  tkm  -1000)) 

"Error  :  more  than  one  legs  are  out  of  kinematic  limit"))) 


(defmethod  (robot  : slow-down-robot) 
0 
(send  body  :slow-down)) 


(defmethod  (robot  : speed-up-robot) 
0 
(send  body  : speed-up)) 


(defmethod  (robot  : stable) 
0 
(send  body  :stable  supporting-legs)) 


(defmethod  (robot  :stable_m) 

<> 

(send  body  :stable-m  supporting-legs)) 


(defmethod  (robot  : stable-without) 
(a-leg) 
(send  body  :stable  (remove  a-leg  supporting-legs))) 
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(defmethod  (robot  :update-robot-status) 
0 
(setf  lift-f lag't) 

(setf  lift-able-legs  (send  self  : f ind-lif t-able-legs)  ) 
(setf  place-able-legs  (send  self  : f ind-place-able-legs)  ) 
(setf  supporting-legs  (send  self  : f ind-supporting-legs) ) 
(setf  supporting-p-legs  (send  self  : f ind-supporting-p-legs)  ) 
(mapcar  #' (lambda  (a-leg)  (send  a-leg  :update-tkm-p) )  supporting-p-legs) 
(if  (send  self  :check-tkm-limit-p) 

(send  body  : stop-motion  (send  self  :check-tkm-limit-p) ) 

(send  body  : restore-motion) ) 
(if  (not  (send  self  :check-stability-p) ) 

(send  body  :modif y-command-p) 

(send  body  : restore-command-p) ) 
(send  body  :calculate-motion  joy-command  legs) 

(mapcar  #'  (lambda  (a-leg)  (send  a-leg  : select-foothold) )  place-able-legs) 
(mapcar  #'  (lambda  (a-leg)  (send  a-leg  :update-tkm) )  supporting-legs)) 


(defun  create-terrain  () 

(send  graph-terrain  :create)) 


(defun  kill-terrain () 

(send  graph-terrain  :kill)) 


******************************************************** 


;   prolog  interface  functions 


******************************************************** 


(defun  at_tkm_limit  () 

(send  asv  :at-tkm-limit) ) 


(defun  do_recovery () 

(send  asv  : do-recovery) ) 


(defun  execute_planned_motion () 

(send  asv  : execute-planned-motion) ) 


(defun  graphical_display (), 

(send  asv  :graphical-display) ) 


(defun  has_more_tkm(legl  leg2) 

(send  asv  :has-more-tkm  legl  leg2)  ) 
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(def un  inits () 

(send  asv  :initti)) 


(defun  leg_with_new_f oothold ( ) 

(send  asv  : leg-with-new-f oothold) ) 


(defun  max_sm_leg (a-leg) 

(send  asv  :max-sm-leg  a-leg) ) 


(defun  modify_command ( ) 

(send  asv  :modif y-command) ) 


(defun  read_joystick ( ) 

(send  asv  : read- joystick) ) 


(defun  restore_command () 

(send  asv  : restore-command) ) 


(defun  send_decision (legl  leg2  a-decision) 

(send  asv  :send-decision  legl  leg2  a-decision)) 


(defun  smallest_tkm_leg () 

(send  asv  : smallest-tkm-leg) ) 


(defun  slow_down_robot () 

(send  asv  : slow-down-robot ) ) 


(defun  speed_up_robot ( ) 

(send  asv  : speed-up-robot ) ) 


(defun  stable_p() 
(send  asv  :  stable)) 


i 
(defun  stable_p_m() 
(send  asv": stable  m)  ) 


(defun  stable_without (a-leg) 

(send  asv  : stable-without  a-leg)) 
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(defun  update_robot_status ( ) 

(send  asv  : update-robot-status) ) 
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(f s : add-logical-pathname-host  "robot"  "root5" 

'  (("kwak"  "supermac : kwak : ")  )  ) 
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(f s :add-logical-pathname-host  "robot"  "root5" 

'(("kwak"  "supermac:kwak:"))) 
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; ; ;  -*-  Mode : Common-Lisp;  Package :USER;  Base: 10  -*- 
•••a**************************************************************** 

r 

;   top  level  motion  planning  coordinator 
.a****************************************************************** 


(defun  my-monitor  (&rest  args) 

(let  ( (x  (mapcar  #'my-output  args))) 
(if  (remove  nil  x) 
(my-print  x) ) ) 
t) 

(defun  my-output (arg) 

(cond  ( (typep  arg  'leg)  (send  arg  :name)) 
( (typep  arg  'atom)  arg) 
((typep  arg  'list)  (cons  (my-output  (car  arg)) 

(my-output  (cdr  arg) ) ) ) 
(t  'error) ) ) 


(defmacro  retract (predicate  Soptional  (argument  t)  ) 
'(cond  ((not  (boundp  (quote  , predicate) ) ) 

nil) 
((and  , predicate  (equal  , argument  '?)) 

(setf  , predicate  (cdr  , predicate)) 

t) 
((member  , argument  , predicate  :test  'equal) 

(setf  , predicate  (remove  , argument  , predicate  :count  1) ) 

t) 
(t  nil))) 


(defmacro  asserta (predicate  ^optional  (argument  t) ) 
'(cond  ((not  (boundp  (quote  , predicate) ) ) 
(setf  , predicate  nil) 

(setf  , predicate  (cons  , argument  , predicate) ) ) 
(t  (setf  , predicate  (cons  , argument  , predicate) ))) ) 

(defmacro  assertz (predicate  Soptional  (argument  t) ) 
Mcond  ((not  (boundp  (quote  ,  predicate)  )  ) 
(setf  , predicate  nil) 

(setf  , predicate  (append  , predicate  (list  , argument) )) ) 
(t  (setf  , predicate  (append  , predicate  (list  , argument) )))) ) 


(defmacro  match (predicate  Soptional  (argument  t) ) 
'(cond  ((not  (boundp  (quote  , predicate) ) ) 
nil) 

((member  ,  argument  , predicate  :test  'equal) 
t) 
(t  nil) ) ) 


(defmacro  unify (predicate  argument) 

'(cond  ((not  (boundp  (quote  , predicate) ) ) 
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;;;  _*_  Mode : Common-Lisp;  Package :USER;  Base: 10  -*- 
.•A***************************************************************** 

;   top  level  motion  planning  coordinator 
.••••a************************************************************** 


(defun  my-monitor  (&rest  args) 

(let  ( (x  (mapcar  #'my-output  args))) 
(if  (remove  nil  x) 
(my-print  x) ) ) 
t) 

(defun  my-output (arg) 

(cond  ( (typep  arg  'leg)  (send  arg  :name)) 
( (typep  arg  'atom)  arg) 
((typep  arg  'list)  (cons  (my-output  (car  arg)) 

(my-output  (cdr  arg) ) ) ) 
(t  'error)  )  ) 


(defmacro  retract (predicate  &optional  (argument  t)) 
'(cond  ((not  (boundp  (quote  , predicate) ) ) 

nil) 
((and  , predicate  (equal  , argument  '?)) 

(setf  , predicate  (cdr  , predicate)) 

t) 
((member  ,  argument  , predicate  :test  'equal) 

(setf  , predicate  (remove  , argument  , predicate  :count  1) ) 

t) 
(t  nil))) 


(defmacro  asserta (predicate  &optional  (argument  t)) 
1 (cond  ((not  (boundp  (quote  , predicate) ) ) 
(setf  , predicate  nil) 

(setf  , predicate  (cons  , argument  , predicate) ) ) 
(t  (setf  ,  predicate  (cons  , argument  /predicate))))) 


(defmacro  assertz (predicate  &optional  (argument  t) ) 
Mcond  ((not  (boundp  (quote  ,  predicate)  )  ) 
(setf  ,predicate  nil) 

(setf  ,predicate  (append  /predicate  (list  , argument) )) ) 
(t  (setf  /predicate  (append  /predicate  (list  , argument) ))) ) 


(defmacro  match (predicate  ^optional  (argument  t) ) 
1 (cond  ((not  (boundp  (quote  /predicate))) 
nil) 

((member  ,  argument  /predicate  :test  'equal) 
t) 
(t  nil) )) 


(defmacro  unify (predicate  argument) 

Mcond  ((not  (boundp  (quote  /predicate))) 
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nil) 
(t  (setf  , argument  (car  , predicate) ))) ) 


;  robot  :-  initialize,  repeat,  my_loop,  fail. 

;  initialize  :-  inits,  init_ditch_plan. 

;  init_dicth_plan  :-  retract (plan_cycle  (_)) ,  retract (plan_state (_)) ,  fail. 

;  init_ditch_plan  :-  asserta (plan_cycle (1) ) ,  asserta (plan_state (place_legs_in_the_air 

;  my_loop  :-  get_comrnand,  plan,  execute,  !. 

;  get_command  :-  X  is  read_joystick. 

;  plan  :-  ditch_mode,  ditch_plan. 

;  plan  :-  normal_plan. 

;  ditch_mode  :-  ditch_mode (in) .    ; ;  cleared  by  ditch_plan. 

;  ditch_mode  :-  X  is  at_ditch_area,  X  ==  t,  asserta (ditch_mode (in) ) . 

;  execute  :-  execute_motion,  draw_robot,  !. 

;  execute_motion  :-  X  is  execute_planned_motion. 

;  draw_robot  : -  X  is  graphical_display . 


(defun  robot  () 
(create -terra in) 
( robot 1) 
(kill-terrain) ) 


(defun  robotl  () 
(initialize) 
(do  () 

( (not  (my_loop) ) ) ) ) 


(defun  initialize () 
(cond  ( (and  (inits) 

(init_ditch_plan)  ) 
t) 
(t  nil))) 


(defun  init_ditch_plan () 

(cond  ( (and  (not  (setf  ditch_mode  nil) ) 
(not  (setf  plan_cycle  nil) ) 
(not  (setf  plan_state  nil) ) 
(not  (setf  limit_leg  nil) ) 
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(not  (setf  reduce_speed  nil) ) 
(not  (setf  f ront_legs  nil) ) 
(not  (setf  middle_legs  nil) ) 
(not  (setf  rear_legs  nil) ) 
(not  (setf  decision  nil) ) 
(asserta  plan_cycle  1) 

(asserta  plan_state  ' place_legs_in_the_air) ) 
t) 
(t  nil))) 


(defun  my_loop() 

(process -a  How- schedule) 
(cond  ( (and  (get_command) 
(plan) 
(execute) ) 
t) 
(t  nil) ) ) 


(defun  get_command  () 

(cond  (t  (read_joystick)  ) 
(t  nil))) 


(defun  plan  () 

(cond  ( (and  (ditch_mode) 
(ditch_plan) ) 
t) 

(  (normal_j?lan) 
t) 
(t  nil) ) ) 


(defun  ditch_mode  0 

(cond  ((match  ditch_mode  'in) 
t) 
( (and  (at_ditch_area) 

(asserta  ditch_mode  'in)) 
t) 
(t  nil) ) ) 


(defun  execute  () 

(cond  ( (and  (execute_motion) 
(draw_robot) ) 
t) 
(t  nil))  ) 

i 
(defun  execute_motion  () 

(cond  (t  (execute_planned_motion)  t) 
(t  nil) )) 


(defun  draw_robot() 

(cond  (t  (graphical_display)  t) 
(t  nil)  )  ) 
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********************************************************** 


Normal  Plan 


********************************************************** 


;  normal_plan  :-  update_robot_state,  check_tkm_limit, 
;        leg_plan,  body_plan,  generate_decision,  ! . 

;  update_robot_state  :-  X  is  update_robot_status . 

check_tkm_limit  :-  A_leg  is  at_tkm_limit,  A_leg  \==  nil, 

asserta (limit_leg (A_leg,  lift)  )  . 
check_tkm_limit . 

;  leg_plan  :-  lift_a_leg. 

;  leg_plan  :-  exchange_legs . 

;  leg_plan  :-  stable. 

;  leg_plan  :-  place_a_leg. 

;  leg_plan  :-  wait_f or_legs . 

;  stable  :-  Condition  is  stable_p,  Condition  ==  t. 

lift_a_leg  :-  stable,  A_leg  is  smallest_tkm_leg,  A_leg  \==  nil, 
Condition  is  stable_without (A_leg) ,  Condition  ==  t, 
asserta (decision (A_leg,_, lift) ) . 

exchange_legs  :-  stable,  LegA  is  smallest_tkm_leg,  LegA  \==  nil, 
LegB  is  max_sm_leg (LegA) ,  LegB  \==  nil, 
Condition  is  has_more_tkm (LegB,  LegA)  , 
Condition  ==  t, 
asserta (decision (LegA, LegB, exchange)  )  . 

;  place_a_leg  :-  A_leg  is  max_sm_leg (_) ,  A_leg  \==  nil, 
;  asserta (decision (A_leg,_, place) ) . 


;  wait_f or_legs 
;  wait_f or_legs 
;  wait_for_legs 


-  try_new_f oothold. 

-  recovery,  asserta (reduce_speed) . 

-  asserta (reduce_speed) ,  restore_limit_leg. 


;  try_new_f oothold  :-  A_leg  is  leg_with_new_f oothold,  A_leg  \==  nil, 
;  asserta (decision (A_leg,_, place) ) . 

;  recovery  :-  A_leg  is  do_recovery,  A_leg  \==  nil, 

;  asserta (decision (A_leg,_, place) ) ,  restore_limit_leg. 

;  restore_limit_leg  :-  retract (limit_leg (A_leg,  lift)  )  . 

;  restore_limit_leg. 


(defun  normal_plan  () 

(cond  ( (and  (update_robot_state) 
(check  tkm  limit) 
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(leg_plan) 
(body_plan) 

(my-monitor  limit_leg  decision  reduce_speed) 
(generate_decision) ) 
t) 
(t  nil) ) ) 


(defun  update_robot_state () 
(cond  ( (update_robot_status) 
t) 
(t  nil) ) ) 


(defun  check_tkm_limit 0 
(let  ((leg)) 

(cond  (  (setf  leg  (at_tkm_limit) ) 

(asserta    lirnit_leg    (list    'lift    leg)) 
t) 

(t   t) 
(t   nil)))) 


(defun    leg_plan() 

;  OR  tree  becomes  regular  "cond"  statement 
(cond  ( (lift_a_leg)   t) 

( (exchange_leg)   t) 

{(stable)   t) 

( (place_a_leg)   t) 

( (wait_f or_legs)  t) 

(t  nil))) 


(defun  stable () 

(cond  ( (stable_p)  t) 
(t  nil))) 


(defun  lift_a_leg() 
(let  ((leg)) 

(cond  ( (and  (stable) 


(setf  leg  (smallest_tkm_leg) ) 

(stable_without  leg) 

(asserta  decision  (list  'lift  leg))) 


(t  nil)))) 


(defun  exchange_leg () 
(let  ((lega)  (legb) ) 
(cond  ( (and  (stable) 

(setf  lega  (smallest_tkm_leg) ) 
(setf  legb  (max_sm_leg  lega) ) 
(has_more_tkm  legb  lega) 

(asserta  decision  (list  'exchange  lega  legb))) 
t) 
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(defun  place_a_leg ( ) 
(let  ((leg)) 

(cond  ( (and  (setf  leg  (max_sm_leg  nil)  ) 

(asserta  decision  (list  'place  leg))) 
t) 
(t  nil) ) ) ) 


(defun  wait_for_legs () 

;  OR 

(cond  ( (try_new_footholds)   t) 

((recovery)  (asserta  reduce_speed)   t) 

( (asserta  reduce_speed)  (restore_limit_leg)   t) 

(t   nil))) 


(defun  try_new_footholds () 
(let  ((leg)) 

(cond  ( (and  (setf  leg  (leg_with_new_foothold) ) 

(asserta  decision  (list  'place  leg))) 
t) 
(t  nil)) ) ) 

(defun  recovery () 
(let  ((leg)) 

(cond  ( (setf  leg  (do_recovery) ) 

(asserta  decision  (list  'place  leg)) 
(restore_limit_leg) 
t) 
(t  nil))) ) 


(defun  restore_limit_leg () 
;  OR 

(cond  ( (and  (unify  limit_leg  leg) 

(retract  limit_leg  leg) ) 
t) 

(t  t) 
(t  nil))) 


********************************************************** 


Ditch  Plan 


********************************************************** 


ditch_plan  :-  ditch_plan._done,  retract (ditch_mode (in) ) ,  idle_cycle, 
ditch_plan  :-  cycle_planner . 


**********   Cycle  planner  ************ 
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;  ditch_plan_done  :-  plan_cycle ( 6) ,  retract (plan_cycle ( 6) ) ,  asserta (plan_cycle (1) ) , 
;  prepare_next_ditch_plan . 

;  prepare_next_ditch_plan  :-  move. 

;  cycle_planner  :-  one_cycle_done,  plan_cycle (N) ,  Nl  is  N+l,  retract (plan_cycle (N) )  ,  as: 

ta (plan_cycle (Nl) ) , 

;  idle_cycle. 

;  cycle_planner  :-  plan_cycle. 


:*********   plan  Cycle  dispatcher  ************ 

;  one_cycle_done  :-  plan_state (one_plan_cycle_done) ,  retract (plan_state (one_plan_cycle_c 
e)), 

;  initialize_plan_state . 

;  plan_cycle  :-  plan_cycle (1) ,  update_robot_state,  ditch_plan_cycle_l,  bodyjplan,  genera 

_decision, ! . 

;    plan_cycle    :-   plan_cycle (2) ,    update_robot_state,    ditch_jplan_cycle_2,    body_plan,    genera 

_decision, ! . 

;  plan_cycle  :-  plan_cycle (3) ,  update_robot_state,  ditch_plan_cycle_3,  body_plan,  genera 

_decision, ! . 

;  plan_cycle  :-  plan_cycle (4) ,  update_robot_state,  ditch_plan_cycle_4,  body_plan,  genera 

_decision, ! . 

;  plan_cycle  :-  plan_cycle (5) ,  update_robot_state,  ditch_plan_cycle_5,  body_plan,  genera 

_decision, ! . 

;  idle_cycle  :-  update_robot_state,  body_plan,  generate_decision,  !. 


(defun  ditch_plan  () 

(cond  ( (and  (ditch_plan_done) 

(retract  ditch_mode  'in) 
(idle_cycle) ) 
t) 

( (cycle_planner) 
t) 
(t  nil))) 


********** 


Cycle  planner  ************ 


(defun  ditch_plan_done  () 

(cond  ( (and  (match  plan_cycle  6) 

(retract  plan_cycle  6) 
(asserta  plan_cycle  1) 
(prepare_next_ditch_plan) ) 
t) 
(t  nil) ) ) 


(defun  prepare_next_ditch_plan  () 
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(cond  ( (move) 
t) 
(t  nil) ) ) 


(defun   cycle_j)lanner    {) 

(cond    ( (and    (one_cycle_done) 

(unify  plan_cycle  N) 
(retract  plan_cycle  N) 
(asserta  plan_cycle  (+  N  1)  ) 
(idle_cycle) ) 
t) 

( (plan_cycle) 
t) 
(t  nil))) 


**********   plan  Cycle  Dispatcher  ************ 


(defun  one_cycle_done  () 

(cond  ((and  (match  plan_state  ' one_plan_cycle_done) 

(retract  plan_state  ' one_plan_cycle_done) 
(initialize_plan_state) ) 
t) 
(t  nil))) 


(defun  plan_cycle  () 

(cond  ( (and  (match  plan_cycle  1) 

(update_robot_state) 

(ditch_plan_cycle_l) 

(body_plan) 

(my-monitor  plan_cycle  plan_state  decision  reduce_speed) 

(generate_decision) ) 
t) 
((and  (match  plan_cycle  2) 

(update_robot_state) 

(ditch_plan_cycle_2) 

(body_plan) 

(my-monitor  plan_cycle  plan_state  decision  reduce_speed) 

(generate_decision) ) 
t) 
( (and  (match  plan_cycle  3) 

(update_robot_state) 

(ditch_plan_cycle_3) 

(body_plan) 

(my-monitor  plan_cycle  plan_state  decision  reduce_speed) 

(generate_decision) ) 
t) 
( (and  (match  plan_cycle  4) 

(update_robot_state) 

(ditch_plan_cycle_4) 

(body_jplan) 

(my-monitor  plan_cycle  plan_state  decision  reduce_speed) 

(generate_decision) ) 
t) 
( (and  (match  plan_cycle  5) 

(update  robot  state) 
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(ditch_plan_cycle_5) 
(body_plan) 

(my-monitor  plan_cycle  plan_state  decision  reduce_speed) 
(generate_decision) ) 
t) 
(t  nil) ) ) 


(defun  idle_cycle  0 

(cond  ( (and  (update_robot_state) 
(body_j?lan) 

(my-monitor  plan_cycle  plan_state  decision  reduce_speed) 
(generate_decision) ) 
t) 
(t  nil) ) ) 


**********   cycles   ********* 


;  initialize_plan_state  :-  asserta (plan_state (start)  )  . 


;  ditch_plan_cycle_l  :-  plan_state (start) ,  retract (plan_state (start )) ,  asserta  (plan_stat 
place_legs_in_the_air) ) , 

place_legs_in_the_air (back_middle_legs) . 

-  place_legs_in_the_air (back_middle_legs) . 

-  back_middle_legs (f orward_rear_legs) . 

-  f orward_rear_legs (f orward_middle_legs) . 

-  forward_middle_legs (f orward_f ront_legs) . 

-  forward_f ront_legs (lif t_middle_legs_and_move) . 

-  lift_middle_legs_and_move (one_plan_cycle_done) . 


;  ditch_plan_cycle_l 

;  ditch_plan_cycle_l 

;  ditch_plan_cycle_l 

;  ditch_plan_cycle_l 

;  ditch_plan_cycle_l 

;  ditch_plan_cycle_l 


;  ditch_plan_cycle_2 
back_middle_legs) )  , 

;  ditchj>lan_cycle_2 
;  ditch_plan_cycle_2 
;  ditch_plan_cycle_2 


-  plan_state (start) ,  retract (plan_state (start) ) ,  asserta  (plan_stat 

back_middle_legs (f orward_rear_legs) . 

-  back_middle_legs (f orward_rear_legs) . 

-  f orward_rear_legs (f orward_middle_legs) . 

-  f orward_middle_legs (one_plan_cycle_done) . 


;  ditch_plan_cycle_3  :-  plan_state (start) ,  retract (plan_state (start )) ,  asserta (plan_stat 
move_f orward_f ront_legs) ) , 

move_forward_f ront_legs (move_forward_middle_legs) . 


;  ditch_pl£1n_cycle_3 
;  ditch__plan_cycle_3 
;  ditch_plan_cycle_3 


-  move_f orward_f ront_legs (move_back_middle_legs) . 

-  move_back_middle_legs (move_f orward_rear_legs) . 

-  move_f orward_rear_legs (one_plan_cycle_done) . 


;  ditch_plan_cycle_4  :-  plan_state (start) ,  retract (plan_state (start) ) ,  asserta (plan_stat 

move_forward_middle_legs) ) , 

;  move_forward_middle_legs (one_plan_cycle_done) . 

;  ditch_plan_cycle_4  :-  move_f orward_middle_legs (one_plan_cycle_done) . 


;  ditch_plan_cycle_5  :-  plan_state (start) ,  retract (plan_state (start) ) ,  asserta  (plan_stat 
move_forward_f ront_legs) ) , 
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;  ditch__plan_cycle_5 
;  ditch_plan_cycle_5 
;  ditch_plan_cycle_5 


move_f orward_f ront_legs (move_f  orward_middle_legs ) 

-  rnove_f orward_f ront_legs (move_back_middle_legs) . 

-  move_back_middle_legs (move_f orward_rear_legs) . 

-  move_f orward_rear_legs (one_plan_cycle_done) . 


.***************   Cycles  **************** 


(defun  initialize_plan_state  () 

(cond  (  (asserta  plan_state  'start) 
t) 
(t  nil) )) 


(defun  ditch_plan_cycle_l  () 

(cond  ((and  (match  plan_state  'start) 

(retract  plan_state  'start) 

(asserta  plan_state  ' place_legs_in_the_air) 
(place_legs_in_the_air  ' back_middle_legs) ) 
t) 

( (place_legs_in_the_air  'back_middle_legs) 
t) 

( (back_middle_legs  ' f orward_rear_legs) 
t) 

( (forward_rear_legs  ' f orward_middle_legs) 
t) 

( (f orward_middle_legs  ' f orward_f ront_legs) 
t) 

( (f orward_f ront_legs  ' lif t_middle_legs_and_move) 
t) 

( (lif t_middle_legs_and_move  ' one_plan_cycle_done) 
t) 
(t  nil))) 


(defun  ditch_plan_cycle_2 () 

(cond  ((and  (match  plan_state  'start) 

(retract  plan_state  'start) 
(asserta  plan_state  'back_middle_legs) 
(back_middle_legs  ' f orward_rear_legs) ) 
t) 

( (back_middle_legs  ' forward_rear_legs) 
t) 

( (f orward_rear_legs  ' f orward_middle_legs) 
t) 

( (f orward_middle_legs  ' one_plan_cycle_done) 
t) 
(t  nil))) 


(defun  ditch_plan_cycle_3  () 

(cond  ((and  (match  plan_state  'start) 

(retract  plan_state  'start) 

(asserta  plan_state  'move_forward_f ront_legs) 
(move_forward_f ront_legs  'move_back_middle_legs)  ) 
t) 

( (move_forward_front_legs  'move_back_middle_legs) 
t) 

( (move_back_middle_legs  'move_forward_rear_legs) 
t) 
( (move_forward_rear_legs  ' one_plan_cycle_done) 
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t) 
(t  nil) ) ) 


(defun  ditch_plan_cycle_4  () 

(cond  ((and  (match  plan_state  'start) 

(retract  plan_state  'start) 

(asserta  plan_state  ' move_f orward_middle_legs) 
(rnove_f  orward_middle_legs  ' one_plan_cycle_done) ) 
t) 

( (move_f orward_middle_legs  ' one_plan_cycle_done) 
t) 
(t  nil) )) 


(defun  ditch_plan_cycle_5  () 

(cond  ((and  (match  plan_state  'start) 

(retract  plan_state  'start) 

(asserta  plan_state  ' move_f orward_f ront_legs) 
(move_f orward_f ront_legs  'move_back_middle_legs) ) 
t) 

( (move_f orward_f ront_legs  'move_back_middle_legs) 
t) 

( (move_back_middle_legs  'move_f orward_rear_legs) 
t) 

( (move_f orward_rear_legs  ' one_plan_cycle_done) 
t) 
(t  nil))) 


**************  states  *************** 


;  back_middle_legs (Next_State)  :-  plan_state (back_middle_legs)  ,  back_middle_legs_done, 

retract (plan_state (back_middle_legs) ) ,  asserta (plan_st 

e (Next_State) ) , 

stop. 
back_middle_legs (Next_State)  :-  plan_state (back_middle_legs) ,  do_back_middle_legs, 

stop. 


;  forward_f ront_legs (Next_State)  :-  plan_state (f orward_f ront_legs) ) ,  f orward_f ront_legs_ 
ne, 

retract (plan_state (forward_f ront_legs) ,  asserta (plan 
tate (Next_State) ) , 

i         stop. 
;  forward_f ront_legs (Next_State)  :-  plan_state (forward_f ront_legs) ) ,  do_forward_f ront_le 

r 

stop. 


;  f orward_middle_legs (Next_State)  :-  plan_state (f orward_middle_legs) ,  f orward_middle_lec 
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done, 

;  retract (plan_state (f orward_middle_legs) ,  asserta ( 

_state (Next_State) ) , 

;  stop. 

;  f orward_middle_legs (Next_State)  :-  plan_state (f orward_middle_legs)  ,  do_f orward_middl 

gs, 

stop. 


;  f orward_rear_legs (Next_State)  :-  plan_state (f orward_rear_legs) ,  f orward_rear_legs_do 

retract (plan_state (f orward_rear_legs) ,  asserta (plan 

te(Next_State) ) , 

stop . 

;  f orward_rear_legs (Next_State)  :-  plan_state (f orward_rear_legs)  ,  do_f orward_rear_legs 

;  stop. 


lift_middle_legs_and_move (Next_State)  :-  plan_state (lif t_middle_legs_and_move) ,  move 
e,  stop, 

retract  (plan_state  (lif  t_middle_legs_and_mo\i 
asserta (plan_state (Next_State) ) . 
lift_middle_legs_and_move (Next_State)  :-  plan_state (lift_middle_legs_and_move)  ,  do_l 
middle_legs ,  move. 


move_back_middle_legs (Next_State)  :-  plan_state (move_back_middle_legs) ,  move_back_mi 
legs_done, 

retract (plan_state (move_back_rniddle_legs) ) ,  ass 
(plan_state (Next_State) ) . 

move_back_middle_legs (Next_State)  :-  plan_state (move_back_middle_legs) ,  do_move_back 
dle_legs . 


;  move_forward_f ront_legs (Next_State)  :-  plan_state (move_forward_f ront_legs)  ,  move_for 
_f  ront_legs_done, 

retract (plan_state (move_f orward_f  ront_legs)  )  , 
erta (plan_state (Next_State) ) . 

;  move_f orward_f ront_legs (Next_State)  :-  plan_state (move_f orward_f ront_legs) ,  do_move_ 
ard_f ront_legs . 


;  move_f orward_middle_legs (Next_State)  :-  plan_state (move_f orward_mlddle_legs) ,  move_f 

rd_middle_legs_done , 

;  retract (plan_state (move_f orward_f  ront_legs)  ) 

serta (plan_state (Next_State) ) . 

;  move_f orward_middle_legs (Next_State)  :-  plan_state (move_f orward_middle_legs)  ,  do_mov 

rward_middle_legs . 


;  move_f orward_rear_legs (Next_State)  :-  plan_state (move_f orward_rear_legs) ,  move_forwa 
iddle_legs_done , 

retract (plan_state (move_forward_rear_legs)  )  ,  a 
ta  (plan_state (Next_State) )  . 

;  move_f orward_rear_legs (Next_State)  :-  plan_state (move_f orward_rear_legs) ,  do_move_fo 
d_rear_legs . 
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;  place_legs_in_the_air (Next_State)  :-  plan_state (place_legs_in_the_air) ,  place_legs_in 

e_air_done, 

;  retract (plan_state (place_legs_in_the_air) ) ,  asse: 

(plan_state (Next_state) ) , 

stop. 
;    place_legs_in_the_air (Next_State)     :-   plan_state (place_legs_in_the_air) ,    do_place_legs 
the    air,     stop. 


**************    states 


*************** 


(defun  back_middle_legs  (next_state) 

(cond  ((and  (match  plan_state  ' back_middle_legs) 
(back_middle_legs_done) 
(retract  plan_state  ' back_middle_legs) 
(asserta  plan_state  next_state) 
(stop) ) 
t) 

((and  (match  plan_state  'back_middle_legs) 
(do_back_middle_legs ) 
(stop)) 
t) 
(t  nil) ) ) 


(defun  f orward_f ront_legs  (next_state) 

(cond  ((and  (match  plan_state  ' forward_f ront_legs) 
( f o  rwa  rd_f  r ont_legs_done ) 
(retract  plan_state  ' f orward_f ront_legs) 
(asserta  plan_state  next_state) 
(stop) ) 
t) 

((and  (match  plan_state  ' f orward_f ront_legs) 
(do_f orward_f ront_legs) 
(stop) ) 
t) 
(t  nil)) ) 


(defun  forward_middle_legs  (next_state) 

(cond  ((and  (match  plan_state  ' forward_middle_legs) 
( f orward_middle_legs_done ) 
(retract  plan_state  ' forward_middle_legs) 
(asserta  plan_state  next_state) 
-  (stop)) 
t) 

((and  (match  plan_state  ' f orward_middle_legs) 
( do_f o  rwa  rd_middle_legs ) 
(stop) ) 
t) 
(t  nil) ) ) 
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(defun  forward_rear_legs  (next_state) 

(cond  ((and  (match  plan_state  ' f orward_rear_legs) 
(f orward_rear_legs_done) 
(retract  plan_state  '  f orward_rear_legs) 
(asserta  plan_state  next_state) 
(stop) ) 
t) 

((and  (match  plan_state  ' f orward_rear_legs) 
( do_f o  rwa  r d_re  a  r_legs ) 
(stop) ) 
t) 
(t  nil) )) 


(defun  lift_middle_legs_and_move  (next_state) 

(cond  ((and  (match  plan_state  ' lif t_middle_legs_and_move) 
(move_done) 
(stop) 

(retract  plan_state  ' lif t_middle_legs_and_move) 
(asserta  plan_state  next_state) ) 
t) 

((and  (match  plan_state  ' lif t_middle_legs_and_mo ve ) 
( lif t_middle_legs ) 
(move) ) 
t) 
(t  nil) ) ) 


(defun  move_back_middle_legs  (next_state) 

(cond  ( (and  (match  plan_state  'move_back_middle_legs) 
(move_back_middle_legs_done ) 
(retract  plan_state  'move_back_middle_legs) 
(asserta  plan_state  next_state) ) 
t) 
( (and  (match  plan_state  'move_back_middle_legs) 

(do_move_back_middle_legs) ) 
t) 
(t  nil)  )  ) 


(defun  move_forward_f ront_legs  (next_state) 

(cond  ( (and  (match  plan_state  'move_forward_f ront_legs) 
(move_forward_f ront_legs_done) 
-  (retract  plan_state  'move_f orward_f ront_legs) 
(asserta  plan_state  next_state) ) 
t) 
( (and  (match  plan_state  'move_f orward_f ront_legs) 

(do_move_f orward_f ront_legs) ) 
t) 
(t  nil) ) ) 
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(defun  move_f orward_middle_legs  (next_state) 

(cond  ((and  (match  plan_state  'rnove_f orward_middle_legs) 
(move_f orward_middle_legs_done) 
(retract  plan_state  'move_f orward_middle_legs) 
(asserta  plan_state  next_state) ) 
t) 
((and  (match  plan_state  ' move_f orward_middle_legs) 

(do_move_f orward_middle_legs) ) 
t) 
(t  nil))) 


(defun  move_f orward_rear_legs  (next_state) 

(cond  ((and  (match  plan_state  'move_f orward_rear_legs) 
(move_f orward_rear_legs_done) 
(retract  plan_state  'move_f orward_rear_legs) 
(asserta  plan_state  next_state) ) 
t) 
( (and  (match  plan_state  'move_f orward_rear_legs) 

(do_move_forward_rear_legs)  ) 
t) 
(t  nil))) 


(defun  place_legs_in_the_air  (next_state) 

(cond  ((and  (match  plan_state  ' place_legs_in_the_air) 
(place_legs_in_the_air_done) 
(retract  plan_state  ' place_legs_in_the_air) 
(asserta  plan_state  next_state) 
(stop) ) 
t) 

((and  (match  plan_state  ' place_leg3_in_the_air) 
(do_place_legs_in_the_air) 
(stop) ) 
t) 
(t  nil) ) ) 


****************   State  Executors  ******************** 


;  move_back_middle_legs_done  :-  all_middle_legs_lifted,  all_middle_legs_placed, 

clear_middle_lifted_memory,  clear_move_memory,  stop. 

;  do_move_back_middle_legs  :-  all_middle_legs_lifted,  move_done,  stop,  place_middle_leg 

ack. 

;  do_move_back_middle_legs  :-  all_middle_legs_lifted,  move. 

;  do_move_back_middle_legs  :-  lift_middle_legs,  stop. 
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move_f orward_f ront_legs_done  :-  all_f ront_legs_lif ted,  all_f ront_legs_placed, 

clear_f ront_lif ted_memory,  clear_move_memory,  stop, 


do_move_f orward_f ront_legs 
do_move_f orward_f ront_legs 
do  move  forward  front_legs 


-  all_f ront_legs_lif ted,  move_done,  stop,  place_f ront_lec 

-  all_f ront_legs_lif ted,  move. 

-  lift_f ront_legs,  stop. 


;  move_f orward_mlddle_legs_done  :-  all_middle_legs_lif ted,  all_middle_legs_placed, 

clear_middle_lifted_memory,  clear_move_memory,  stop, 

;  do_move_forward_middle_legs  :-  all_middle_legs_lif ted,  move_done,  stop,  place_middle 

s . 

;  do_move_f orward_middle_legs  :-  all_middle_legs_lifted,  move. 

;  do_move_f orward_middle_legs  :-  lif t_middle_legs,  stop. 


move_f orward_rear_legs_done  :-  all_rear_legs_lif ted,  all_rear_legs_placed, 

clear_rear_lifted_memory,  clear_move_memory,  stop. 

do_move_f orward_rear_legs  :-  all_rear_legs_lif ted,  move_done,  stop,  place_rear_legs 
do_move_forward_rear_legs  :-  all_rear_legs_lif ted,  move. 
do_move_f orward_rear_legs  :-  lift_rear_legs,  stop. 


move  :-  asserta (resume_movement) 
stop  :-  asserta (stop_movement) . 


clear_move_memory  :-  retract (move (done)  )  . 
c 1 e  a  r_mo ve_memo  r y . 


move_done 
move_done 
move  done 


-  move (done) . 

-  X  is  at_tkm_limit,  X  \==  nil,  asserta (move (done) ) . 

-  X  is  at_stability  limit,  X  \==  nil,  asserta (move (done) ) 


;  back_middle_legs_done  :-  all_middle_legs_lif ted,  all_middle_legs_placed,  clear_middl< 
fted_memory,  clear_move_memory . 

do_back_middle_legs  :-  all_middle_legs_lifted,  place_middle_legs_back. 
do_back_middle_legs  :-  lif t_middle_legs . 

all_middle_legs_lifted  :-  middle_legs (lifted)  . 

all_middle_legs_lifted  :  -  X  is  both_middle_legs_lifted,  X  ==  t,  asserta  (middle_legs  (J 
ed)  )  .  , 

all_middle_legs_placed  :-  X  is  both_middle_legs_placed,  X  ==  t . 

clear_middle_lifted_memory  :-  retract (middle_legs (lifted)  )  . 

place_middle_legs_back  :-  A_leg  is  placable_middle_leg,  A_leg  \==  nil,  asserta (decisi 
A_leg,_,place_back) ) . 
;  place_middle_legs_back. 
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;  lift_middle_legs  :-  A_leg  is  lif table_middle_leg,  A_leg  \==  nil,  asserta (decision (A_le 

_,  lift)). 

;  lift  middle  legs. 


;  f orward_f ront_legs_done  :-  all_f ront_legs_lif ted,  all_f ront_legs_placed,  clear_f ront_l 
ted_memory,  clear_move_memory . 

do_f orward_f ront_legs  :-  all_f ront_legs_lif ted,  place_f ront_legs . 
do_f orward_f ront_legs  :-  lif t_f ront_legs . 

all_f ront_legs_lifted  :-  f ront_legs (lif ted) . 

all_f ront_legs_lifted  :-  X  is  both_f ront_legs_lif ted,  X  ==  t,  asserta (front_legs (lif te 
)  . 

all_f ront_legs_placed  :-  X  is  both_f ront_legs_placed,  X  ==  t . 

clear_f ront_lifted_memory  :-  retract (front_legs (lif ted) ) . 

place_f ront_legs  :-  A_leg  is  placable_f ront_leg,  A_leg  \==  nil,  asserta  (decision (A_leg 
, place) )  . 

place_f ront_legs . 

lif t_f ront_legs  :-  A_leg  is  lif table_f  ront_leg,  A_leg  \==  nil,  asserta  (decision (A_leg, 
lift))  . 
;  lif t_f ront_legs . 


;  forward_middle_legs_done  :-  all_middle_legs_lif ted,  all_middle_legs_placed,  clear_midd 
_lif ted_memory,  c lea  r_mo ve_memo  ry . 

do_f orward_middle_legs  :-  all_middle_legs_lif ted,  place_middle_legs . 
do_forward_middle_legs  :-  lif t_middle_legs . 

place_middle_legs  :-  A_leg  is  placable_middle_leg,  A_leg  \==  nil,  asserta (decision (A_l 
,_,place)  )  . 
;  place_middle_legs . 


;  forward_rear_legs_done  :-  all_rear_legs_lif ted,  all_rear_legs_placed,  clear_rear_lif te 
memory,  clear_move_memory . 

do_f orward_rear_legs  :-  all_f ront_legs_lifted,  place_rear_legs . 
do_f orward_rear_legs  :-  lif t_rear_legs . 

all_rear_legs_lifted  :-  rear_legs (lifted) . 

all_rear_legs_lifted  : -  X  is  both_rear_legs_lifted,  X  ==  t,  asserta (rear_legs (lifted)  ) 

all_rear_legs_placed  :-  X  is  both_rear_legs_placed,  X  ==  t. 

clear_rear_lifted_memory  :-  retract (rear_legs (lifted) ) . 

place_rear_legs  :-  A_leg  is  placable_rear_leg,  A_leg  \==  nil,  asserta (decision (A_leg,_ 
lace) )  . 

place_rear_legs . 

lift_rear_legs  :-  A_leg  is  iiftable_rear_leg,  A_leg  \==  nil,  asserta (decision (A_leg,_, 
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ft)  )  . 

;  lift  rear  legs. 


do_lif t__middle_legs  :-  lif t_middle_legs 


;  place_legs_in_the_air_done  :-  X  is  all_legs_placed,  X  ==  t 

place_legs  :-  A_leg  is  placable_leg,  A_leg  \==  nil, 

assert a (decision (A_leg,_, place) ) . 
place_legs . 


****************   State  Executors  ******************** 


(defun  move_back_middle_legs_done  () 
(cond  ( (and  (all_middle_legs_lifted) 
(all_middle_legs_placed) 
(clear_middle_lifted_memory) 
(clear_move_memory) 
(stop) ) 
t) 
(t  nil))) 


(defun  do_move_back_middle_legs  () 
(cond  ( (and  (all_middle_legs_lifted) 
(move_done) 

(place_middle_legs_back) 
(stop) ) 
t) 
( (and  (all_middle_legs_lif ted) 

(move) ) 
t) 
( (and  (lift_middle_legs) 

(stop) ) 
t) 
(t  nil))) 


(defun  move_forward_f ront_legs_done  () 
(cond  ( (and  (all_f ront_legs_lifted) 
(all_f ront_legs_placed) 
( c lea r_f  r ont_l  i f t ed_memo r y ) 
(clear_move_memory) 
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(stop) ) 
t) 
(t  nil) ) ) 


(defun  do_move_f orward_f ront_legs  () 
(cond  ( (and  (all_f ront_legs_lif ted) 
(move_done) 
(place_f ront_legs ) 
(stop) ) 
t) 
( (and  (all_front_legs_lifted) 

(move) ) 
t) 
( (and  (lift_f ront_legs) 

(stop) ) 
t) 
(t  nil) )) 


(defun  move_f orward_middle_legs_done  () 
(cond  ( (and  (all_middle_legs_lif ted) 
(all_middle_legs_placed) 
(clear_middle_lif ted_memory) 
( c lea  r_mo ve_memo  r y ) 
(stop) ) 
t) 
(t  nil) )) 


(defun  do_move_f orward_middle_legs  () 
(cond  ( (and  (all_middle_legs_lif ted) 
(mo ve_done ) 
(place_middle_legs) 
(stop) ) 
t) 
( (and  (all_middle_legs_lif ted) 

(move) ) 
t) 
( (and  (lift_middle_legs) 

(stop) ) 
t) 
(t  nil))) 


(defun  move_forward_rear_legs_done  () 
(cond  ( (and  (all_rear_legs_lifted) 
(all_rear_legs_placed) 
-  (clear_rear_lifted_memory) 
( clea  r_move_memory ) 
(stop) ) 
t) 
(t  nil) ) ) 
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(defun  do_move_f orward_rear_legs  () 
(cond  ( (and  (all_rear_legs_lifted) 
(rnove_done) 
(place_rear_legs) 
(stop) ) 
t) 
( (and  (all_rear_legs_lifted) 

(move) ) 
t) 
((and  (lift_rear_legs) 

(stop) ) 
t) 
(t  nil))) 


(defun  move  () 

(cond  (  (asserta  resume_movement) 
t) 
(t  nil))) 


(defun  stop  () 

(cond  (  (asserta  stop_movement) 
t) 
(t  nil)) ) 


(defun  clear_move_memory  () 
(cond  ((retract  move  'done) 
t) 

(t  t) 
(t  nil))) 


(defun  move_done  () 

(cond  ((match  move  'done) 
t) 
(  (and  (at_tkm_limit) 

(asserta  move  'done)) 
t) 
( (and  (at_stability_limit) 

(asserta  move  'done)) 
t) 
(t  nil))) 


(defun  back_middle_legs_done  () 

(cond  ( (and  (all_middle_legs_lif ted) 
(all_middle_legs ^placed) 
(clear_middle_lifted_memory) 
(clear_move_memory) ) 
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t) 
(t   nil) ) ) 


(defun  do_back_middle_legs  () 

(cond  ( (and  (all_middle_legs_lifted) 
(place_middle_legs_back) ) 
t) 

( (lift_middle_legs) 
t) 
(t  nil) ) ) 


(defun  all_middle_legs_lifted  () 
(cond  ( (match  middle_legs  ' lifted) 
t) 
( (and  (both_middle_legs_lifted) 

(asserta  middle_legs  'lifted)) 
t) 
(t  nil)) ) 


(defun  all_middle_legs_placed  () 
(cond  ( (both_middle_legs_placed) 
t) 
(t  nil)) ) 


(defun  clear_middle_lifted_memory  () 
(cond  ((retract  middle_legs  'lifted) 
t) 
(t  nil)) ) 


(defun  place_middle_legs_back  () 
(let  (leg) 

(cond  ( (and  (setf  leg  (placable_middle_leg) ) 

(asserta  decision  (list  'place_back  leg))) 
t) 

(t  t) 
(t  nil) ) ) ) 


(defun  lift_middle_legs  () 
(let  (leg) 

(cond  ( (and  (setf  leg  (lif table_middle_leg) ) 

(asserta  decision  (list  'lift  leg))) 
t) 

(t  t) 
(t  nil) ) ) ) 


(defun  forward_f ront_legs_done  () 
(cond  ( (and  (all_f ront_legs_lifted) 
(all_f ront_legs_placed) 
( c le a r_f r ont_l i f t ed_memo ry ) 
(clear_move_memory) ) 
t) 
(t  nil) ) ) 
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(defun  do_f orward_f ront_legs  () 

(cond  ( (and  (all_f ront_legs_lif ted) 
(place_f ront_legs) ) 
t) 

( (lift_front_legs) 
t) 
(t  nil) ) ) 


(defun  all_f ront_legs_lif ted  () 
(cond  ((match  front_legs  'lifted) 
t) 
( (and  (both_f ront_legs_lif ted) 

(asserta  front_legs  'lifted)) 
t) 
(t  nil) ) ) 


(defun  all_f ront_legs_placed  () 
(cond  ( (both_f ront_legs_placed) 
t) 
(t  nil))) 


(defun  clear_f ront_lif ted_memory  () 
(cond  ((retract  front_legs  'lifted) 
t) 
(t  nil))) 


(defun  place_f ront_legs  0 
(let  (leg) 

(cond  ( (and  (setf  leg  (placable_f ront_leg)  ) 

(asserta  decision  (list  'place  leg))) 
t) 

(t  t) 
(t  nil)))) 


(defun  lift_f ront_legs  () 
(let  (leg) 

(cond  ( (and  (setf  leg  (lif table_f ront_leg) ) 

(asserta  decision  (list  'lift  leg))) 
t) 

(t  t) 
(t  nil)))) 


(defun  forward_middle_legs_done  () 
(cond  ( (and  (all_middle_legs_lifted) 
(all_middle_legs_placed) 
(clear_middle_lifted_memory) 
(clear_move_memory) ) 
t) 
(t  nil) ) ) 
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(defun  do_f orward_middle_legs  () 

(cond  ( (and  (all_middle_legs_lif ted) 
(place_middle_legs )  ) 
t) 

( (lift_middle_legs) 
t) 
(t  nil) ) ) 


(defun  place_middle_legs  () 
(let  (leg) 

(cond  ( (and  (setf  leg  (placable_middle_leg) ) 

(asserta  decision  (list  'place  leg))) 
t) 

(t  t) 
(t  nil) ))) 


(defun  f orward_rear_legs_done  () 
(cond  ( (and  (all_rear_legs_lif ted) 
(all_rear_legs_placed) 
(clear_rear_lif ted_memory) 
(clear_move_memory)  ) 
t) 
(t  nil) ) ) 


(defun  do_f orward_rear_legs  0 

(cond  ( (and  (all_rear_legs_lifted) 
(place_rear_legs) ) 
t) 

( (lift_rear_legs) 
t) 
(t  nil))) 


(defun  all_rear_legs_lifted  () 
(cond  ((match  rear_legs  'lifted) 
t) 
( (and  (both_rear_legs_lifted) 

(asserta  rear_legs  'lifted)) 
t) 
(t  nil) ) ) 


(defun  all_rear_legs_placed  () 
(cond  ( (both_rear_legs_placed) 
t)  , 

(t  nil))) 


(defun  clear_rear_lifted_memory  () 
(cond  ((retract  rear_legs  'lifted) 
t) 
(t  nil) ) ) 
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(defun  place_rear_legs  () 
(let  (leg) 

(cond  ( (and  (setf  leg  (placable_rear_leg) ) 

(asserta  decision  (list  'place  leg))) 
t) 

(t  t) 
(t  nil) ) )  ) 


(defun  lif t_rear_legs  () 
(let  (leg) 

(cond  ( (and  (setf  leg  (liftable_rear_leg) ) 

(asserta   decision    (list    'lift    leg))) 
t) 

(t    t) 
(t   nil)))) 


(defun  do_lift_middle_legs  () 
(cond  ( (lift_middle_legs) 
t) 
(t  nil))) 


(defun  place_legs_in_the_air_done  () 
(cond  ( (all_legs_placed) 
t) 
(t  nil))) 


(defun  do_place_legs_in_the_air ( ) 
(let  (leg) 

(cond  ( (and  (setf  leg  (placable_leg) ) 

(asserta  decision  (list  'place  leg))) 
t) 

(t  t)  , 

(t  nil)))) 
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********************************************************** 

Plan  Libraries 
********************************************************** 


;  body_plan  :-  speed_plan,  tra jectory_plan . 

;  speed_jplan  :-  retract  (reduce_speed)  ,  slow_down. 

;  speed_plan  :-  speed_up. 

;  speed_up  :-  X  is  speed_up_robot . 

;  slow_down  :-  X  is  slow_down_robot . 

;  tra jectory_plan  :-  stable_m,  restore_tra jectory . 

;  tra jectory_plan  :-  modify_tra jectory . 

;  stable_m  : -  Condition  is  stable_p_m,  Condition  ==  t. 

;  restore_tra jectory  :-  X  is  restore_corranand. 


;  modify_tra jectory 
generate_decision 
generate_decision 
generate_decision . 


-  X  is  modify_command. 

-  retract (decision (A_leg, B_leg, A_decision)  )  , 

X  is  send_decision (A_leg, B_leg, A_decision) ,  fail 

-  retract (limit_leg (A_leg, A_decision)  )  , 

X  is  send_decision (A_leg,_, A_decision)  ,  fail. 


(defun  body_plan() 

(cond  ( (and  (speed_plan) 

(tra jectory_plan) ) 
t) 
(t  nil))) 


(defun  speed_plan() 

(cond  ( (and  (retract  reduce_speed) 
(slow_down) ) 
t) 
( (and  (retract  stop_movement) 

(stop_motion) ) 
t) 

( (and  (retract  resume_movement) 
( resume_mot ion ) 
(speed_up) ) 
t) 

( (speed_up)  t) 
(t  nil))) 


(defun  speed_up() 

(cond  (t  (speed_up_robot)  t) 
(t  nil))) 

(defun  slow  down ( ) 
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(cond  (t  (slow_down_robot)  t) 
(t  nil) ) ) 

(defun   tra jectory_plan  ( ) 
;    OR 

(cond    (  (and    (stable_rn) 

(restore_tra jectory)  ) 
t) 

( (modify_tra jectory)  t) 
(t  nil) ) ) 

(defun  stable_m  () 
.  (cond  ( (stable_p_m)  t) 
(t  nil) ) ) 

(defun  restore_tra jectory  ( ) 
(cond  (t  (restore_command)  t) 
(t  nil)  )  ) 

(defun  modify_tra jectory () 
(cond  (t  (modif y_command)  t) 
(t  nil) ) ) 

(defun  generate_decision  ( ) 

(cond  (  (not  (unify  decision  a-decision) ) 
nil) 
( (and  (unify  decision  a-decision) 

(retract  decision  a-decision) 

(print  (list  (second  a-decision)  (third  decision)  (first  decision) ) ) 
(send_decision  (second  a-decision)  (thirdd  decision)  (first  decision) 
(generate_decision) ) 
t))) 

(defun  generate_decision () 
(cond  ( (and  decision 
(not 

(dolist  (a-decision  decision)  (send-one-decision  a-decision) ) ) 
;  dolist  returns  nil 
(not  (setf  decision  nil) ) 
nil)   ;  this  simulates  fail 
t) 
( (and  limit_leg 

(unify  limit_leg  decisionl) 
(send-one-decision  decisionl) 
(retract  limit_leg  '?) 
nil) 
t) 

(t  t) 
(t  nil))) 


(defun  send-one-decision  (decision) 
;  format  (decision  legl  leg2) 
;  lisp  function 

(cond  ((equal  (first  decision)  'exchange) 

(send_decision  (second  decision)  (third  decision)  (first  decision) ) ) 
(t 
(send_dec*ision  (second  decision)  nil  (first  decision)  )  )  ) 
t) 
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;;;  -*-  Mode : Common-Lisp;  Base: 10  -*- 

********************************************************* 

sensor  flavor  definition 
********************************************************* 

(def flavor  sensor (state  owner) 
0 
: initable-instance-variables) 


********************************************************* 


contact-sensor  flavor  definition 


********************************************************* 


(def flavor  contact-sensor () 
(sensor) 

: initable-instance-variables) 


(defmethod  (contact-sensor  :initti) 
(leg-name) 
(setf  state  (send  self  :sensing))) 

(defmethod  (sensor  :contact-p) 
() 
state) 

(defmethod  (sensor  : sensing) 

0 
;  simulation  purpose 
;  graph-terrain  is  object. 
(setf  state 

(let*  ( (leg-pos-wrt-body  (send  (send  owner  :executor) 

:leg-pos-wrt-body) ) 
(leg-pos-wrt -earth 

(to-earth-transf orm  (send  owner  :get-Hl)  leg-pos-wrt-body) ) 
(x-y-pos  (list  (first  leg-pos-wrt-earth) 

(second  leg-pos-wrt-earth) ) ) 
(leg-height  (third  leg-pos-wrt-earth) ) ) 
(if  (<  leg-height  (+  0.07  (send  graph-terrain  :get-height  x-y-pos))) 
t 
nil))) ) 
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; ; ;  -*-  Mode : Common -Lisp;  Base: 10  -*- 
********************************************************* 

stability-calculator  flavor  definition 
********************************************************* 

(def flavor  stability-calculator (safety-margin 

safety-ma rgin-p 
large- safety-margin 
large -safety-ma rgin-p 
recovery- vector 
recovery-vector-p 
owner) 
0 
: initable-instance-variables) 


(defmethod  (stability-calculator  :initti) 
0 
(setf  safety-margin  0.4) 
(setf  safety-margin-p  0.2) 
(setf  large-safety-margin  0.5) 
(setf  large-safety-margin-p  0.4) 
(setf  recovery-vector  ' (0  0  0) ) 
(setf  recovery-vector-p  ' (0  0  0))) 


(defmethod  (stability-calculator  :get-recovery-vector) 
0 
recovery- vector) 


(defmethod  (stability-calculator  :get-recovery-vector-p) 
() 
recovery-vector-p) 


(defmethod  (stability-calculator  : convert-to-recovery-vector) 
(stability-vector) 
(let  ( (sm  (first  stability-vector) ) 

(vect  (second  stability-vector) ) ) 
(cond  ( (<  sm  0) 
nil) 
( (<  sm  0.1) 

(magvect  (/  1  sm)  vect)) 
(t 
(magvect  (/  0.1  (*  sm  sm) )  vect))))) 

(defmethod  (stability-calculator  :more-stable) 
(supporting-legs  H  legl  leg2) 
(let  ( (stabilityl  (send  self  :calculate-stability 

(cons  legl  supporting-legs)  H) ) 
(stability2  (send  self  :calculate-stability 

(cons  leg2  supporting-legs)  H) ) ) 
(if  (>  stabilityl  stability2) 
t 
nil))) 
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(defmethod  (stability-calculator  :stable-m) 
;  predict  H  <=  H10 

(supporting-legs  H) 
(let  (  (stability-vector 

(send  self  :get-stability  supporting-legs  H) ) ) 
(cond  ( (>=  (first  stability-vector) 
large-safety-margin) 
t) 
(t 
(if  (>=  (first  stability-vector)  safety-margin) 
(setf  recovery-vector 

(send  self  :convert-to-recovery-vector  stability-vector)) 
(setf  recovery-vector  ' (0  0  0))) 
nil)))) 


(defmethod  (stability-calculator  :stable-p-m) 
;  present  H  <=  HI 

(supporting-p-legs  H) 
(let*  ((stability-vector 

(send  self  :get-stability  supporting-p-legs  H)  ) 
(st-margin  (first  stability-vector) ) ) 
(cond  ( (>=  st-margin 

large-safety-margin-p) 
t) 
(t 
(setf  recovery-vector-p 

(send  self  : convert-to-recovery-vector  stability-vector)) 
(if  (<  st-margin  safety-margin-p) 

(my-print  (list  'st-p  st-margin))) 
nil))) ) 


(defmethod  (stability-calculator  : stable) 
(supporting-legs  H10) 
(if  (>=  (send  self  rcalculate-stability 

supporting-legs  H10) 
safety-margin) 
t 
nil)  ) 


(defmethod  (stability-calculator  :stable-p) 

(supporting-p-legs  HI) 

(if  (>=  (send  self  :calculate-stability 

supporting-p-legs  HI) 
safety-margin-p) 
t 
nil)) 


(defmethod  (stability-calculator  :calculate-stability) 
(supporting-legs  H) 
(first  (send  self  :get-stability  supporting-legs  H) )  ) 


(defmethod  (stability-calculator  :get-stability) 
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(supporting-legs  K) 
(if  (>=  (counting  supporting-legs)  3) 

(measure-distance  (center-of -gravity  H) 
(convex-hull 

(supporting-points 
supporting-legs) ) ) 
'  (-100.0  (0  0  0) )  )  ) 


(defun  convex-hull  (points) 
;  returns  clockwise-ordered 
;  point  list  of  convex  hull 
(reverse 

(convexl  (car  points)  points 
'(0  0  0)  nil) ) ) 


(defun  convexl  (current-point  points  previous-pt  visited-pts) 
(let*  ( (min-out-pt  (min-out  current-point  previous-pt  points) ) 
(pos  (position  min-out-pt  visited-pts  :test  #'equal))) 
(cond  (pos 

(subseq  visited-pts  0  (+  pos  1) ) ) 
(t  (convexl  min-out-pt 
points 

cur rent -point 
(cons  min-out-pt  visited-pts) ) ) ) ) ) 


(defun  min-out  (current-pt  pv-pt  pts) 
(let*  ( (min-pt  nil) 

(min-angle  100) 
(angle  0) ) 
(dolist  (a-pt  pts) 

(cond  ((not  (equal  a-pt  current-pt)) 

(setf  angle  (turning-angle  (vectsub  current-pt  pv-pt) 

(vectsub  a-pt  current-pt) ) 
(cond  ( (<  angle  min-angle) 
(setf  min-pt  a-pt) 
(setf  min-angle  angle) ) ) ) ) ) 
min-pt) ) 


(defun  turning-angle (vect  new-vect) 

;  2  D  space  clock-wise  turning  angle 

;  Neither  vect  should  not  be  zero  vector. 

(let*  ( (vectl-0  (list  (first  vect)  (second  vect)  0)) 

(vect2-0  (list  (first  new-vect)  (second  new-vect)  0)) 
(normal-vect  (crossprod  vectl-0  vect2-0)) 
(polarity  (>  (third  normal-vect)  0)  ) 
(value  (/  (dotprod  vectl-0  vect2-0) 

(*  (magnitude  vectl-0) 
(magnitude  vect2-0) ) ) ) 
(angle  0) ) 
(if  (>=  value  1) 

(setf  value  1) ) 
(if  (<=  value  -1) 

(setf  value  -1) ) 
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(setf  angle  (acos  value) ) 
(if  polarity 

(-  (*  2  pi)  angle) 

angle) ) ) 


(defun  center-of-gravity  (H) 

;  center-of -body  is  represented  wrt  earth  coordinate. 
(let  (  (x  (aref  H  0  3) ) 
(y  (aref  H  1  3) ) ) 
(list  x  y) ) ) 
;  center-of -body  can  be  changed  in  future. 


(defun  find-slope  (first-point  second-point) 

(let  ( (del-x  (-  (car   second-point)  (car   first-point) ) ) 
(del-y  (-  (cadr  second-point)  (cadr  first-point)))) 
(if  (>  (abs  del-x)  0.0000001) 
(/  del-y  del-x)  . 
nil) )) 


(defun  infinite-case  (x  a-line) 
(list  x 

(+  (*  (car  a-line)  x)  (cadr  a-line)))) 


(defun  intersection-point  (a-line  b-line) 

;  Returns  list  (x  y) .       Line  is  list  (slope   crossing-point-of-axis) 
(cond  ( (null  (car  a-line) )  (infinite-case  (cadr  a-line)  b-line) ) 

( (null  (car  b-line) )  (infinite-case  (cadr  b-line)  a-line) ) 

(t   (normal-case  a-line  b-line) ) ) ) 


(defun  in-side-of-convex-hull  (center-point  first-points  second-points) 
(do*  ( (first-points   first-points   (cdr  first-points) ) 
(second-points  second-points  (cdr  second-points) ) 
(in-side-flag  T) ) 
(  (null  first-points)  in-side-flag) 
(if  (test-out-side  (car  first-points)  center-point  (car  second-points) ) 
(setf  in-side-flag  nil) ) ) ) 


(defun  line  (slope  point) 
(if  slope  ' 

(list  slope  (-  (second  point)  (*  slope  (first  point)))) 

(list  slope  (first  point) ) ) ) 
;  When  slope  is  infinitive,  return  with  x-axis  crossing  point  instead  of 
;  y-axis  crossing  point. 


(defun  measure-distance  (center-point   convex-points) 
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;  convex-points  is  a  list  of  points 
;  point  is  a  list  (x  y  z) . 

(let*  (  (first-points  convex-points) 

(second-points  (append  (cdr  convex-points) 
(list  (car  first-points) ) ) ) ) 
(if  (in-side-of-convex-hull  center-point  first-points  second-points) 
(start -measure  center-point  first-points  second-points) 
'  (-10.0  (0  0  0) ) ) ) ) 

;  center-of-gravity  is  out-side  of  support  pattern 


(defun  normal-case  (a-line  b-line) 
(let*  (  (al  (car  a-line) ) 
(bl  (cadr  a-line) ) 
(a2  (car  b-line) ) 
(b2  (cadr  b-line) ) 
(x   (/  (-  bl  b2)  (-  a2  al) ) ) 
(y   (+  (*  al  x)  bl) ) ) 
(list  x  y) ) ) 


(defun  point-distance  (center-point  first-point  second-point) 
;  returns  distance  and  vector  between  cross-pt  and  center-pt 
(let*  ( (slopel  (find-slope  first-point  second-point) ) 
(slope2  (right-angle  slopel) ) 
(cross-pt  (intersection-point  (line  slopel  first-point) 

(line  slope2  center-point) ) ) 
(del-vect  (vectsub  center-point  cross-pt  ) ) 
(distance  (magnitude  del-vect) ) ) 
(list  distance  (list  (first  del-vect)  (second  del-vect)  0.0)))) 


(defun  right-angle  (slope) 

(cond  ((null  slope)  0.0)  ;  infinitive  input  slope 

( (<  (abs  slope)  0.0000001)  nil)  ;  zerop  slope 
(t  (/  (-  1)  slope)) ) ) 


(defun  start-measure  (center-point  first-points  second-points) 
(do*  ( (first-points  first-points  (cdr  first-points)  ) 

(second-points  second-points  (cdr  second-points) ) 

(min-distance  10000.0  min-distance)    ;  infinte  dummy  number  10000.0 
(min-direction  nil)  (dis-dir  nil) ) 
( (null  first-points)   (list  min-distance  min-direction) ) 
(setf  dis-dir  (point-distance  center-point 

(car  first-points)  (car  second-points) ) ) 
(cond  ( (<  (first  dis-dir)  min-distance) 

(setf  min-distance  (first  dis-dir)) 
(setf  min-direction  (second  dis-dir) ) ) ) ) ) 

(defun  supporting-points  (legs) 
(mapcar  #' (lambda  (leg) 

(send  leg  : foothold)) 
legs) ) 
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(defun  test-out-side  (first-point  second-point  third-point) 
(let*  ((a  (-  (cadr  first-point)   (cadr  third-point))) 
(b  (-  (car   third-point)   (car   first-point)  )  ) 
(c  (-  (+  (*  a  (car  third-point))  (*  b  (cadr  third-point))))) 
(decision  (+  (*  a  (car   second-point)) 
(cadr  second-point)) 


(if 


(>=  decision 
T 
nil)) ) 


(*  b 
c))) 
0.0) 
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; ;  -*-  Mode : Common-Lisp;  Base : 10  -*- 
******************************************************************* 

stop-body  definition 
******************************************************************* 


(defflavor  stop-body  (stop-body-motion-flag) 
(body) 
:  initable-instance-variables) 


(defmethod  (stop-body  rafter  :initti) 
0 
(setf  stop-body-motion-flag  nil) ) 


(defmethod  (stop-body  : stop-body-motion) 
0 
(setf  stop-body-motion-flag  t) ) 


(defmethod  (stop-body  : restore-body-motion) 
0 
(setf  stop-body-motion-flag  nil) ) 


(defmethod  (stop-body  :calculate-motion) 
(joystick-command  legs) 
(setf  joy-command  joystick-command) 
(cond  ( (equal  support -plane-clock  10) 
(setf  estimated-support -plane 

(send  support-plane-estimator  :get-plane  legs)) 
(setf  support -plane-clock  0))) 
(setf  support-plane-clock  (+  support -plane-clock  1) ) 
(cond 

( (or  stop-motion-flag  stop-body-motion-flag  (null  modify-vector-p) ) 
(send  body-controller  : control 
'(0  0  0) 

0  estimated-support-plane) ) 
(modify-vector-p 
(send  body-controller  : control 

(vectadd  joy-command  (send  self  :get-modify-vector-p) ) 
deceleration-factor  estimated-support-plane) ) 
(t 
(control  body-controller 

(vectadd  joy-command  (send  self  :get-modify-vector)  ) 
deceleration-factor  estimated-support-plane) )  )  ) 
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;;  -*-  Mode :Common-Lisp;  Base: 10  -x- 
********************************************************* 

support-plane-estimator  flavor  definition 
********************************************************* 


(def flavor  support-plane-estimator (owner) 
0 

) 


(defmethod  (support-plane-estimator  :initti) 
0 

) 


(defmethod  (support-plane-estimator  :get-plane) 
(legs) 
(let*  ( (f ootholds-for-estimation  (get-footholds  legs) ) 

(constants  (get-constants  f ootholds-for-estimation) ) ) 
(make-plane-f rom-coef f icient  constants) ) ) 


********************************************************* 


support -plane-estimator . get-plane 


******************************************************** 


(defun  add-points  (points) 

;  returns  a  list  (number-of-points   sum-of -points) . 
(do  ( (points  points  (cdr  points) ) 
(i  0  (+  i  1)  ) 
(sum-vect  ' (0  0  0) ) ) 
((null  points)  (list  i  sum-vect)) 
(setf  sum-vect  (vectadd  (car  points)  sum-vect)))) 


(defun  average-point  (points) 

(let*  ( (num-s-sum-  'ect  (add-points  points)) 

(number-of-points  (first  num-& -sum-vect) ) 
(sum-vect  (second  num-& -sum-vect) ) ) 
(if  (>  number-of-points  0) 

(magvect  (/  1  number-of-points)  sum-vect) 

(print  "Error  in  finding  average-point  of  estimate  plane")))) 


(defun  get-aO  (bar-point  al) 

(let*  ( (x-bar  (first  bar-point)) 
(z-bar  (third  bar-point) ) ) 
(-  z-bar  (*  al  x-bar) ) ) ) 
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(defun  get-al  (points  bar-point  common-denominator) 
;  returns  al  which  is  sum  in  this  function. 
(do*  ((points  points  (cdr  points)) 
(sum  0) 

(x  nil)   (x-bar  (first  bar-point) ) 
(z  nil)   (z-bar  (third  bar-point))) 
((null  points)  (/  sum  common-denominator)) 
(setf  x  (first  (car  points))) 
(setf  z  (third  (car  points) ) ) 
(setf  sum  (+  sum  (*  (-  x  x-bar)  (-  z  z-bar)))))) 


(defun  get-a2  (points  bar-point  common-denominator) 
;  returns  a2  which  is  sum  in  this  function, 
(do*  ( (points  points  (cdr  points) ) 
(sum  0) 

(x  nil)   (x-bar  (first   bar-point) ) 
(y  nil)   (y-bar  (second  bar-point) ) ) 
( (null  points)  (/  sum  common-denominator) ) 
(setf  x  (first   (car  points) ) ) 
(setf  y  (second  (car  points) ) ) 
(setf  sum  (+  sum  (*  (-  x  x-bar)  (-  y  y-bar)))))) 


(defun  get-a3  (bar-point  a2) 

(let*  ((x-bar  (first   bar-point)) 
(y-bar  (second  bar-point) ) ) 
(-  y-bar  (*  a2  x-bar) ) ) ) 


(defun  get-a4  (points  aO  al  a2  a3) 

(let*  ( (number-of-points  (counting  points) ) 
(yr  (get-yr  points  a2  a3) ) 
(zr  (get-zr  points  aO  al) ) 
(yr-bar  (get-yr-bar  yr  number-of-points) ) 
(zr-bar  (get-zr-bar  zr  number-of-points))) 
(do  ( (yr      yr      (cdr  yr) ) 
(zr      zr      (cdr  zr) ) 
(numerator  0)  (a-yr  0)  (a-zr  0) 
(denominator  0)) 
( (null  yr)  (/  numerator  denominator) ) 
(setf  a-yr  (first  yr)  ) 
(setf  a-zr  (first  zr)  ) 

(setf  numerator    (+  numerator    (*  (-  a-yr  yr-bar)  (-  a-zr  zr-bar)))) 
(setf  denominator  (+  denominator  (*  (-  a-yr  yr-bar)  (-  a-yr  yr-bar))))))) 


(defun  get -common-denominator  (points  bar-point) 
(do*  ( (points  points  (cdr  points) ) 
(sum  0)  ' 

(x  nil) 

(x-bar  (first  bar-point) ) ) 
( (null  points)  sum) 
(setf  x  (first  (car  points) ) ) 
(setf  sum  (+  sum  (*  (-  x  x-bar)  (-  x  x-bar)))))) 
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(defun  get-constants  (points) 

(let*  ((bar-point  (average-point  points)) 

(common-denominator  (get-common-denominator  points  bar-point) ) 
(al  (get-al  points  bar-point  common-denominator)  ) 
(a2  (get-a2  points  bar-point  common-denominator)  ) 
(aO  (get-aO  bar-point  al) ) 
(a3  (get-a3  bar-point  a2)) 
(a4  (get-a4  points  aO  al  a2  a3) ) ) 
(list  aO  al  a2  a3  a4) ) ) 


(defun  get-footholds  (legs) 
(do*  ( (legs  legs  (cdr  legs) ) 
(footholds  nil) 
(a-leg  nil) ) 
( (null  legs)  footholds) 
(setf  a-leg  (car  legs) ) 
(if  (send  a-leg  : foothold) 

(setf  footholds  (cons  (send  a-leg  : foothold)  footholds))))) 


(defun  get-yr  (points  a2  a3) 

(do*  ( (points  points  (cdr  points) ) 
(yr  nil) 
(x   nil) 
(y   nil)) 
( (null  points)  (reverse  yr) ) 
(setf  x  (first   (car  points) ) ) 
(setf  y  (second  (car  points) ) ) 
(setf  yr  (cons  (-  y  a2  (*  a3  x) )  yi ) ) ) ) 


(defun  get-yr-bar  (yr  number-of -points) 
(do  ( (yr  yr  (cdr  yr) ) 
(yr-bar  0) ) 
((null  yr)  (/  yr-bar  number-of-points) ) 
(setf  yr-bar  (+  yr-bar  (first  yr) ) ) ) ) 


(defun  get-zr  (points  aO  al) 

(do*  ( (points  points  (cdr  points) ) 
(zr  nil) 
(x   nil) 
(z   nil)) 
((null  points)  (reverse  zr) ) 
(setf  x  (first   (car  points) ) ) 
(setf  z  (third   (car  point.!))) 
(setf  zr  (cons  (-  z  aO  (*  al  x) )  zr) ) ) ) 


(defun  get-zr-bar  (zr  number-of-points) 
(do  ( (zr  zr  (cdr  zr) ) 
(zr-bar  0) ) 
((null  zr)  (/  zr-bar  number-of-points)) 
(setf  zr-bar  (+  zr-bar  (first  zr) ) ) ) ) 
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(defun  make-plane-f rom-coef f icient  (constants) 
(let*  ( (aO  (first   constants)) 
(al  (second  constants) ) 
(a2  (third   constants) ) 
(a3  (fourth   constants)) 
(a4  (fifth   constants)) 
(a   (-  (*  a4  a3)  al) ) 
(b   (-  a4)  ) 
(c   1) 

(d   (-  (*  a2  a4)  aO) ) 

(unit-normal  (normalize-vector  (list  a  b  c) ) ) 
(dis  (/  d  (magnitude  (list  a  b  c) ) ) ) ) 
(list  unit-normal  dis) ) ) 
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;  ;  ;  -*-  Mode  :  Corr^r.or.-Lisp;  Base:  10  -*- 
.•••a**************************************************** 

terrain-regulator  flavor  definition 
.•A****************************************************** 


(def flavor  terrain-regulator (body-rotate-rate-x  body-rotate-rate-y 

body-trans-rate-z   old-body- rot at e-rate-x 
o Id-body- rotate-r at e-y  old-body-trans-rate-z 
gain  min-height  max-height 
etal  eta2  min-eta  max-eta) 
(regulator) 
:init able -instance -variables) 


(defmethod  (terrain-regulator  rinitti) 
0 
(setf  gain  5) 

(setf  min-eta  0.0000001) 
(setf  max-eta  0.4363) 
(setf  min-height  4.4) 
(setf  max-height  5.4) 
(setf  etal  min-eta) 
(setf  eta2  0.5236) 


0  degree 

2  5  degrees 

4.4  feet 

5.4  feet 

0  degree 

30  degree 


(setf  body-rotate-rate-x  0.0) 

(setf  body-rotate-rate-y  0.0) 

(setf  body-trans-rate-z  0.0) 

(list  body-rotate-rate-x  body-rotate-rate-y  body-trans-rate-z)  ) 


(defmethod  (terrain-regulator  : do-terrain-regulation) 

(k-gamma-delta-height) 
;  k-gamma-delta-height  is  ((k.x  k.y  k.z)  gamma  delta-height), 
(let*  ( (k      (first   k-gamma-delta-height)  ) 
(gamma  (second  k-gamma-delta-height)  ) 
(delta-height  (third  k-gamma-delta-height) ) 
(body-rotate-rate-x-n  (*  gain  (first   k)  gamma)) 
(body-rotate-rate-y-n  (*  gain  (second  k)  gamma) ) 
(body-trans-rate-z-n   (*  gain  delta-height))) 
(setf  body-rotate-rate-x 
(send  self  rlimitor 

(send  self  : filter  body-rotate-rate-x-n  body-rotate-rate-x) 
0.1)) 
(setf  body-rotate-rate-y 
(send  self  :limitor 

(send  self  : filter  body-rotate-rate-y-n  body-rotate-rate-y) 
0.1)) 
(setf  body-trans-rate-z 

(send  self  rlimitor 

(send  self  : filter  body-trans-rate-z-n  body-trans-rate-z) 
1))) 
(list  body-rotate-rate-x  body-rotate-rate-y  body-trans-rate-z) ) 


(defmethod  (terrain-regulator  :eta-function) 
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(eta) 
(let  ((slope  (/  (-  max-eta  min-eta)  (-  eta2  etal)))) 
(+  min-eta  (*  slope  (-  eta  etal))))) 


(defmethod  (terrain-regulator  :get-k-gamma-by-slope) 
(plane  H) 
(let*  ( (plane-rpt-body  (plane-transform  plane  H) ) 
(height  (cadr  plane-rpt-body) ) 
(eta  (arc-cos  (third  (car  plane) ) ) ) 
(k-gamma-desired-height  nil) ) 
(setf  k-gamma-desired-height 

(cond  ( (<  eta  etal)  (send  self  :low-slope  plane)) 

( (<  eta  eta2)  (send  self  :mid-slope  eta  plane  H) ) 
(T  (send  self  :high-slope  plane  H) ) ) ) 

(list  (first  k-gamma-desired-height) 
(second  k-gamma-desired-height) 
(-  (third  k-gamma-desired-height)  height) ) ) ) 


(defmethod  (terrain-regulator  : height-function) 
(eta) 
(let  ((slope  (/  (-  max-height  min-height)  (-  eta2  etal)))) 
(-  max-height  (*  slope  (-  eta  etal) ) ) ) ) 


(defmethod  (terrain-regulator  :high-slope) 
(plane  H) 
(let*  ( (plane-unit-normal  (first  plane) ) 
(a  (first   plane-unit-normal) ) 
(b  (second  plane-unit-normal) ) 
(m  (sqrt  (+  (*  a  a)  (*  b  b)  ) ) ) 
(desired-eta  max-eta) 
(desired-height  min-height) 

(desired-body-plane  (list  (list  (*  (/  a  m)  (sin  desired-eta)) 

(*  (/  b  m)  (sin  desired-eta) ) 
(cos  desired-eta))  0.0)) 
(desired-body-plane-in-body  (plane-transform  desired-body-plane  H) ) 
(unit-normal-body-plane  (first  desired-body-plane-in-body) ) 
(al  (first   unit-normal-body-plane) ) 
(bl  (second  unit-normal-body-plane) ) 
(cl  (third   unit-normal-body-plane) ) 
(ml  (sqrt  (+  (*  al  al)  (*  bl  bl) ) ) ) 
(k   (if  (=  ml  0) 

(list  0  0  0) 

(list  (/  (-  bl)  ml)  (/  al  ml)  0))) 
(gamma  (arc-cos  cl) ) ) 
(list  k  gamma  desired-height) ) ) 


(defmethod  (terrain-regulator  :limitor) 
(vel  max-vel) 
(if  (>=  (abs  vel)  max-vel) 
(if  (>  vel  0) 
max-vel 
(-  max-vel) ) 
vel)  ) 
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(defmethod  (terrain-regulator  : low-slope) 
(plane) 
(let*  ((unit-normal  (first  plane)) 
(a  (first   unit-normal) ) 
(b  (second  unit-normal)  ) 
(c  (third   unit-normal) ) 
(m  (sqrt  (+  (*  a  a)  (*  b  b) ) ) ) 
(k.a  nil) 
(k.b  nil) 

(gamma  (arc-cos  c) ) 
(desired-height  max-height) ) 
(if  (=  m  0.0) 

(setf  k.a  0.0  k.b  0.0) 

(setf  k.a  (/  (-  b)  m)   k.b  (/  a  m) ) ) 
(list  (list  k.a  k.b  0.0)  gamma  desired-height))) 


(defmethod  (terrain-regulator  :mid-slope) 
(eta  plane  H) 
(let*  (  (plane-unit-normal  (first  plane) ) 
(a  (first   plane-unit-normal) ) 
(b  (second  plane-unit-normal) ) 
(m  (sqrt  (+  (*  a  a)  (*  b  b) ))) 
(desired-eta  (send  self  :eta-f unction  eta)) 
(desired-height  (send  self  rheight-function  eta) ) 
(desired-body-plane  (list  (list  (*  (/  a  m)  (sin  desired-eta) ) 

(*  (/  b  m)  (sin  desired-eta)) 
(cos  desired-eta))  0.0)) 
(desired-body-plane-in-body  (plane-transform  desired-body-plane  H) ) 
(unit-normal-body-plane  (first  desired-body-plane-in-body) ) 
(al  (first   unit-normal-body-plane)) 
(bl  (second  unit-normal-body-plane) ) 
(cl  (third   unit-normal-body-plane) ) 
(ml  (sqrt  (+  (*  al  al)  (*  bl  bl) ) ) ) 
(k   (if  (=  ml  0) 

(list  0  0  0) 

(list  (/  (-  bl)  ml)  (/  al  ml)  0))) 
(gamma  (arc-cos  cl)  )  ) 
(list  k  gamma  desired-height)  )  ) 


(defmethod  (terrain-regulator  : regulate) 
(estimated-support -plane  H) 
(let  ( (k-gamma  (send  self  :get-k-gamma -by-slope  estimated-support -plane  H) ) ) 
(send  self  :do-terrain-regulation  k-gamma) ) ) 


(defmethod  (terrain-regulator  : restore) 
0 
(setf  body-rotate-rate-x  old-body-rotate-rate-x) 
(setf  body-rotate-rate-y  'old-body-rotate-rate-y) 
(setf  body-trans-rate-z   old-body-trans-rate-z) 
(list  body-rotate-rate-x  body-rotate-rate-y  body-trans-rate-z) ) 


(defmethod  (terrain-regulator  :save) 
0 
(setf  old-body-rotate-rate-x  body-rotate-rate-x) 
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(setf  old-body-rotate-rate-y  body-rotate-rate-y) 
(setf  old-body-trans-rate-z   body-trans-rate-z) 
) 
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;;  -*-  Mode : Common-Lisp;  Base: 10  -*- 
******************************************************************* 

test-overlap-leg  definition 
******************************************************************* 


(defflavor  test-overlap-leg  () 
(overlap-leg) 
) 


(def method  (test-overlap-leg  : change-to-back-foothold) 
() 
(setf  foothold  (first  foothold-list)) 
(setf  tkm  (first  tkm-list) ) ) 
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; ;  -*-  Mode : Common-Lisp;  Base: 10  -*- 
******************************************************************* 

test-overlap-robot  definition 
******************************************************************* 


(defflavor  test-overlap-robot  () 
(over lap- robot) 
) 


(defmethod  (tes 
0 
(send  graph-a 
(setf  vision- 
(send  vision- 
(setf  joystic 
(send  joystic 
(empty-queue 
(setf  lift-fl 
(let  ((H)) 
(setf  body 
(setf  H  (se 
(setf  legs 


t-overlap-robot  rinitti) 

sv  :init-data) 

system  (make-instance  'ditch-vision-system  :owner  self)) 

system  :initti) 

k  (make-instance  ' joystick) ) 

k  :reset) 

lift-queue) 

ag  t) 


:owner  self) ) 


(make-instance  'body 
nd  body  :initti)) 
(list 

(make-instance  'test 
(make-instance  'test 
(make-instance  'test 
(make-instance  'test 
(make-instance  'test 
(make-instance  'test 
)) 
(mapcar  #' (lambda  (a-leg)  (send  a-leg  :initti  H) )  legs)) 


:-overlap-leg 
:-overlap-leg 
:-overlap-leg 
-overlap-leg 
;-overlap-leg 
;-overlap-leg 


:name 
:name 
:name 
:name 
:name 
:name 


'legl 
'leg2 
'leg3 
'leg4 
'leg5 
'leg6 


rowner  self) 

:owner  self) 

rowner  self) 

: owner  self) 

: owner  self) 

:owner  self) 


(defmethod  (test-overlap-robot  :send-decision) 
(legl  leg2  a-decisicn) 
(cond  ((equal  a-decision  'exchange) 

(send  legl  : send-decision  a-decision) 
(send  leg2  : send-decision  'place) 
(send  legl  :send-exchange  leg2) ) 
( (equal  a-decision  'place_back) 
(send  legl  : change-to-back-foothold) 
(send  legl  : send-decision  'place)) 
(t 
(send  legl  : send-decision  a-decision)))) 


:t,km)) 
:tkm))) 


(defmethod  (test-overlap-robot 
(legl  leg2) 
(let  ((tkml  (send  legl 
(tkm2  (send  leg2 
(cond  (  (null  tkm2) 
t) 

(  (null  tkml) 
nil) 
(  (>  tkml 

tkm2))))) 


:has-more-tkm) 
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(defmethod  (test-overlap-robot  : lif table-leg) 
(leg) 
(cond  ((member  leg  lift-able-legs  :test  #'equal) 
leg) 
(t  nil) )) 


(defmethod  (test-overlap-robot  :placable-leg) 
(leg) 
(cond  ((and  (member  leg  place-able-legs  :test  #'equal) 
(send  leg  :has-foothold-p)  ) 
leg) 
(t  nil))) 


(defmethod  (test-overlap-robot  : lif table-front-leg) 
0 
(cond  ((send  self  : liftable-leg  (first  legs))) 
((send  self  :liftable-leg  (second  legs))) 
(t  nil))) 


(defmethod  (test-overlap-robot  :placable-f ront-leg) 
0 
(cond  ((send  self  :placable-leg  (first  legs))) 
((send  self  :placable-leg  (second  legs))) 
(t  nil))) 


(defmethod  (test-overlap-robot  :both-f ront-legs-placed) 

0 
;  If  one  of  front  legs  has  not  foothold,  then  this  will  fail! 

(cond  ((and  (member  (first  legs)  supporting-p-legs  :test  #'equal) 

(member  (second  legs)  supporting-p-legs  :test  #'equal)) 
t) 
(t  nil))) 


(defmethod  (test-overlap-robot  :both-f ront-legs-lifted) 
0 
(cond  ((and  (not  (member  (first  legs)  supporting-p-legs  :test  #'equal)) 

(not  (member  (second  legs)  supporting-p-legs  :test  f'equal))) 
t) 
(t  nil))) 


(defmethod  (test-overlap-robot  : lif table-rear-leg) 
0 
(cond  ((send  self  :liftable-leg  (fifth  legs))) 
((send  self  : liftable-leg  (sixth  legs))) 
(t  nil))  ) 
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(defmethod  (test-overlap-robot  :placable-rear-leg) 
0 
(cond  ((send  self  :placable-leg  (fifth  legs))) 
((send  self  :placable-leg  (sixth  legs))) 
(t  nil) ) ) 


(defmethod  (test-overlap-robot  :both-rear-legs-placed) 

0 
;  If  one  of  rear  legs  has  not  foothold,  then  this  will  fail! 

(cond  ((and  (member  (fifth  legs)  supporting-p-legs  :test  t'equal) 
(member  (sixth  legs)  supporting-p-legs  :test  #'equal)) 
t) 
(t  nil))) 


(defmethod  (test-overlap-robot  :both-rear-legs-lif ted) 
0 
(cond  ((and  (not  (member  (fifth  legs)  supporting-p-legs  :test  #'equal)) 
(not  (member  (sixth  legs)  supporting-p-legs  :test  #'equal))) 
t) 
(t  nil)) ) 


(defmethod  (test-overlap-robot  : lif table-middle-leg) 
0 
(cond  ((send  self  :liftable-leg  (third  legs))) 
((send  self  : liftable-leg  (fourth  legs))) 
(t  nil))) 

(defmethod  (test-overlap-robot  : placable-middle-leg) 
0 
(cond  ( (send  self  :placable-leg  (third  legs) ) ) 
((send  self  :placable-leg  (fourth  legs))) 
(t  nil))) 

(defmethod  (test-overlap-robot  : both-middle-legs -placed) 

0 
;  If  one  of  middle  legs  has  not  foothold,  then  this  will  fail! 

(cond  ((and  (member  (third  legs)  supporting-p-legs  :test  #'equal) 

(member  (fourth  legs)  supporting-p-legs  :test  #'equal)) 
t) 
(t  nil))) 

(defmethod  (test-overlap-robot  :both-middle-legs-lifted) 
0 
(cond  ((and  (not  (member'  (third  legs)  supporting-p-legs  .-test  #'equal)) 

(not  (member  (fourth  legs)  supporting-p-legs  :test  #'equal))) 
t) 
(t  nil)) ) 


(defmethod  (test-overlap-robot  : one-placable-leg) 
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0 
(cond  ( (send  self  :placable-f ront-leg) ) 
( (send  self  :placable-middle-leg) ) 
(  (send  self  :placable-rear-leg) ) 
(t  nil)  )  ) 


(defmethod  (test-overlap-robot  : all-legs-placed) 
0 
(cond  ( (and  (send  self  :both-f ront-legs-placed) 
(send  self  :both-middle-legs-placed) 
(send  self  :both-rear-legs-placed)  ) 
t) 
(t  nil)) ) 


(defmethod  (test-overlap-robot  :at-ditch-area) 
0 
(send  vision-system  :on-ditch-area 
(send  self  :get-H10) ) ) 


************************************************************ 


;    Prolog  Interface  Functions 


************************************************************ 


(defun  liftable_f ront_leg  () 

(send  asv  :liftable-f ront-leg) ) 


(defun  placable_f ront_leg  0 

(send  asv  :placable-f ront-leg) ) 


(defun  both_f ront_legs_placed  () 

(send  asv  :both-f ront-legs-placed) ) 


(defun  both_f ront_legs_lifted  () 

(send  asv  :both-f ront-legs-lif ted) ) 


(defun  liftable_rear_leg  () 

(send  asv  :liftable-rear-rleg)  ) 


(defun  placable_rear_leg  () 

(send  asv  :placable-rear-leg) ) 


(defun  both_rear_legs_placed  () 

(send  asv  :both-rear-legs-placed) ) 
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(defun  both_rear_legs_lif ted  () 

(send  asv  :both-rear-legs-lif ted) ) 


(defun  lif table_middle_leg  () 

(send  asv  : lif table-middle-leg) ) 


(defun  placable_middle_leg  () 

(send  asv  :placable-middle-leg) ) 


(defun  both_middle_legs_placed  () 

(send  asv  :both-middle-legs-placed)  ) 


(defun  both_middle_legs_lifted  () 

(send  asv  :both-middle-legs-lif ted)  ) 


(defun  placable_leg  () 

(send  asv  :one-placable-leg) ) 


(defun  all_legs_placed  () 

(send  asv  :all-legs-placed) ) 


(defun  at_ditch_area  () 

(send  asv  :at-ditch-area) ) 
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; ;  -*-  Mode : Common-Lisp;  Base: 10  -*- 
******************************************************* 

tkm-calculator  flavor  definition 
************************  *  *  ***************************** 


(def flavor  tkm-calculator (working-volume  owner) 
0 
: initable-instance-variables) 


(defmethod  (tkm-calculato 
(leg-name) 
(cond  ( (equal  leg-name 
(setf  working-vo 
'  (((  (0  0  1 
(((0  0  1 
( (equal  leg-name 
(setf  working-vo 
'  ((((0  0  1 
(((0  0  1 
( (equal  leg-name 
(setf  working-vo 
'  ((((0  0  1 
(((0  0  1 
(  (equal  leg-name 
(setf  working-vo 
'  ((((0  0  1 
(((0  0  1 
( (equal  leg-name 
(setf  working-vo 
'  ((((0  0  1 
(((0  0  1 
( (equal  leg-name 
(setf  working-vo 
'  ((((0  0  1 
(((0  0  1 

) 
) 


initti) 


'legl) 

lume 

)  3.316) 

(d 

0 

0) 

-8 

0832)  ( 

)  5.7313) 

(  (1 

0 

0) 

-3 

4167)  ( 

'leg2) 

lume 

)  3.316) 

(d 

0 

0) 

-8 

0832)  ( 

)  5.7313) 

(  (1 

0 

0) 

-3 

.4167)  ( 

'leg3) 

lume 

)  3.316) 

(d 

0 

0) 

-2 

.2915)  ( 

)  5.7313) 

(d 

0 

0) 

2 

.2915)  ( 

'leg4) 

lume 

)  3.316) 

(<1 

0 

0) 

-2 

.2915)  ( 

)  5.7313) 

(  (1 

0 

0) 

2 

.2915)  ( 

'leg5) 

lume 

)  3.316) 

(<1 

0 

0) 

3 

.3332)  ( 

)  5.7313) 

(d 

0 

0) 

7 

.8332)  ( 

'leg6) 

lume 

)  3.316) 

(d 

0 

0) 

3 

.3332)  ( 

)  5.7313) 

(d 

0 

0) 

7 

.8332)  ( 

(0  0.9397   0.3420)  -2.569)) 

(0  0.9397  -0.3420)  -2.569))))) 


(0  0.9397   0.3420)   2.569)) 
(0  0.9397  -0.3420)   2.569) ) ))) 


(0  0.9397   0.3420)  -2.569) ) 
(0  0.9397  -0.3420)  -2.569))))) 


(0  0.9397   0.3420)   2.569)) 
(0  0.9397  -0.3420)   2.569))))) 


(0  0.9397   0.3420)  -2.569)) 
(0  0.9397  -0.3420)  -2.569))))) 


(0  0.9397   0.3420)   2.569)) 
(0  0.9397  -0.3420)   2.569)) ) )) 


(defmethod  (tkm-calculator  :find-tkm) 

(a-foothold  body-trans-rate  body-rotate-rate) 
;  a-foothold  is  based  on  body  coordinate 
;  returns  tkm 

(let*  ( (leg-vel-rpt-body 

(get -leg- velocity 

a-foothold  body-trans-rate  body-rotate-rate)  )  ) 
(get-tkm  a-foothold  leg-vel-rpt-body  working-volume) ) ) 


(defun  get-distance  (planes  velocity  leg-position) 
;  global  function  :  plane-distance 
;  before  start,  make  one  plane  list 

(do  ( (planes  (append  (first  planes)  (second  planes) )  (cdr  planes) ) 
(a-tkm  nil) 
(min-tkm  10000) ) 
( (null  planes)  min-tkm) 
(setf  a-tkm  (plane-distance  (car  planes)  velocity  leg-position) ) 
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(if  a-tkm 

(if  (and  (>  a-tlcm  0)  (>  min-tkm  a-tkm)) 
(setf  min-tkm  a-tkm)  ) ) ) ) 


(defun  get-leg-velocity  (pos-rpt-body  body-trans-rate  body-rotate-rate) 
;  returns  leg-velocity-wrt-body 

=  -  (  body-trans-rate  +  body-rotate-rate  X  pos-rpt-body  ) 
(vectsub  '(0  0  0) 

(vectadd  body-trans-rate 

(crossprod  body-rotate-rate  pos-rpt-body) ) ) ) 


(defun  get-tkm  (leg-pos-rpt-body  velocity  working-volume) 
;  global  function  :  magnitude 

;  outside  w.v  returns  nil.   If  speed  is  near  0,  then  returns  1000.0. 
(if  (in-side-volume  leg-pos-rpt-body  working-volume) 
(let  ( (speed  (magnitude  velocity) ) ) 
(if  (<  speed  1/1000) 
1000.0 

(/  (get-distance  working-volume  velocity  leg-pos-rpt-body)  speed))) 
nil)) 


(defun  in-side-volume  (position  planes) 
;  planes  (  (up  front  left)  (back  right  bottom) ) 
(let*  ( (positive-planes  (first  planes) ) 
(negative-planes  (second  planes) ) 
(inside-flag  T) ) 
(dolist  (a-plane  positive-planes) 

(if  (>=  (plane-normal-distance  a-plane  position)  0) 
(setf  inside-flag  nil) ) ) 
(dolist  (a-plane  negative-planes) 

(if  (<=  (plane-normal-distance  a-plane  position)  0) 
(setf  inside-flag  nil) ) ) 
inside-flag) ) 
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; ; ;  -*-  Mode : Common-Lisp ;  Base: 10  -*- 
.**************************•**************************************** 

user  interface  routines 

(defvar  *old-terrain-f ile-name*) 

(defvar  *new-terrain-f ile-name*) 

(defvar  *terrain-slope-type*) 

(defvar  *terrain-slope-data*) 

(defvar  *terrain-slope-angle*) 

(defvar  *terrain-type*) 

(defvar  *obstacle-ratio*) 

(defvar  *random-seed*) 

(defvar  *ok-flag*) 

(defvar  *screen*) 

(defvar  *screen-width*) 

(defvar  *screen-height*) 

(defvar  *new-lisp-listener*) 

(defvar  *ditch-width*) 

(defvar  *ditch-location*) 

(defvar  *ditch-type*) 

(defun  initialize-menu-variables  0 
(setf  *old-terrain-f ile-name*  nil) 
(setf  *new-terrain-f ile-name*  nil) 
(setf  *terrain-slope-type*  'default) 
(setf  *terrain-slope-data*  nil) 
(setf  *terrain-slope-angle*  0) 
(setf  *terrain-type*  'random) 
(setf  *obstacle-ratio*  '25) 
(setf  *random-seed*  '125) 
(setf  *ok-flag*  t) 
(setf  *ditch-width*  6) 
(setf  *ditch-location*  21) 
(setf  *ditch-type*  'no-ditch)) 

(defun  get-old-terrain-file-name () 
(let  ( (file-names 

(mapcar  #' (lambda  (file) 

(list  (f ile-namestring  file)  ' documentation  "Use  an  old  terrain") 
(directory  "robot : kwak . robot .terrain-data; *.*")))) 
(if  file-names 

(w:menu-choose  (cons  '("new-terrain"  :value  nil  : documentation  "Create  a  new  ter 
in") 

file-names) 
: label  "Select  terrain" 
: superior  *new-lisp-listener*) 
(w:menu-choose  ' ( ("new-terrain"  :value  nil  : documentation  "Create  a  new  terrain" 
: label  "Select  terrain" 
: superior  *new-lisp-listener*) ) ) ) 
i 


(defun  get-terrain-slope-type () 
(w:choose-variable- values 

' ( (*terrain-slope-type*  :menu-alist  (("Default"  :value  default) 

("Single  Angle"  : value  single-angle) 
("Manual"  : value  manual) ) ) ) 
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: label  "Choose  terrain  slope  profile" 
:superior  *new-lisp-listener*) 
*terrain-slope-type*) 


(defun  get-terrain-slope-angle ( ) 
(w : choose -variable -values 

'  (  (*terrain-slope-angle*  : documentation  "Input  terrain  slope  angle" 

: constraint  (lambda  (dl  d2  d3  value) 

(cond  ( (>  (abs  value)  30)  "Too  steep  angle"] 


(t  nil))))) 


:  label  "Input  terrain  slope  angle' 
: superior  *new-lisp-listener*) 
* terrain-slope-angle*) 


(defun  get-terrain-slope-data () 

(setf  *terrain-slope-data*  '((15  0)  (30  2))) 
(wr choose-variable-values 

' ( (*terrain-slope-data*  : documentation  "Input  format  ( (xl  hi)  (x2  h2)  ...  )" 

:constraint  (lambda  (dl  c±2  d3  value) 

(cond  ((null  value)  "Please  input  slope") 


(t  nil))))) 


: label  "Input  slope  data" 
: superior  *new-lisp-listener*) 
*ter rain-slope-data*) 


(defun  get-terrain-obstacle-type  () 
(w : choose-variable-values 

' ( (*terrain-type*  :menu-alist  (("Random"  rvalue  random) 

("Manual"  rvalue  manual) ) ) ) 
: label  "Choose  type  of  terrain" 
: superior  *new-lisp-listener*) 
*t err a in-type*) 


(defun  get-terrain-random-data () 
(w: choose-variable-values 

' ( (*obstacle-ratio*  :constraint  (lambda  (dl  d2  d3  value) 

(cond  ( (>  value  90)  "Too  Big") 
( (<  value  0)  "Error") 


(t  nil)))) 


(* random-seed*  rfixnum)) 
: superior  *new-lisp-listener*) 
(list  *obstacle-ratio*  *random-seed*) ) 


t 
(defun  get-ditch-type  () 
(w: choose-variable-values 

' ( (*ditch-type*  :menu-alist  ( ("Add  Ditch"  rvalue  add-ditch) 

("No  Ditch"  rvalue  no-ditch)))) 
r label  "Choose  ditch  option" 
:  superior  *new-lisp-listener*) 
*ditch-type*) 
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(defun  get-ditch-width-location () 
(w: choose- variable- values 

' ( (*ditch-width*  :constraint  (lambda  (dl  d2  d3  value) 

(cond  (  (>  value  7)  "Too  Big") 

((<  value  3)  "Too  Small") 
(t  nil) )) ) 
(*ditch-location*  :constraint  (lambda  (dl  d2  d3  value) 

(cond  ((>  value  32)  "Too  Big") 

( (<  value  15)  "Too  Small") 
(t  nil))))) 
:superior  *new-lisp-listener*) 
(list  *ditch-width*  *ditch-location*) ) 


(defun  user-ok() 
(setf  *ok-flag*  t) 

(w: choose -variable -value s 
'((*ok-flag*  rboolean)) 

: label  "Do  you  like  this  terrain?" 
: superior  *new-lisp-listener*) 
*ok-flag*) 


(defun  user-file-name  0 

(w: choose -variable- values 

' ( (*new-terrain-f ile-name*  :string)) 
: label  "Please  provide  the  Dutput  file  name 
: superior  *new-lisp-listener*) 
*new-ter rain- file-name*) 


(defun  user-save () 

(setf  *ok-flag*  nil) 

(w: choose- variable- values 

'((*ok-flag*  "Save-p"  rboolean)) 

: label  "Do  you  want  to  save  this  terrain?" 

rsuperior  *new-lisp-listener*) 

*ok-flag*) 


(defun  move-and-shape-lisp-listener ( ) 

(setf  *screen*  (send  *terminal-io*  rsuperior)) 

(setf  *screen-width*  (send  *screen*  rwidth) ) 

(setf  *screen-height*  (send  *screen*  rheight) ) 

(setf  *new-lisp-listener*  (make-instance  'w: lisp-listener) ) 

(send  *new-lisp-listener^  : refresh) 

(send  *new-lisp-listener*  :set-size 

(truncate    (*    1.0    *screen-width*) )     (truncate    (*    0.2    *screen-height*) ) ) 
(send   *new-lisp-listener*    : set-position 

0    (truncate    (*    0.8    *screen-height*) ) ) 
(send   *new-lisp-listener*    :set-more-p  nil) 
(send   *new-lisp-listener*    : select)) 


user-interface-t2 .lisp         Thu  Nov  29  11:36:49  1990 


(defun  restore-lisp-listener () 

(send  *new-lisp-listener*  :kill)) 


(defun  my-print  (x) 

(print  x  *new-lisp-listener*)  ) 


(defun  my-read-char-no-hang  () 

(read-char-no-hang  *new-lisp-listener*) ) 
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; ; ;  -*-  Mode : Common-Lisp;  Base: 10  -*- 
•A****************************************************************** 

r 

i 

vision-system  definition 


******************************************************************* 


(defflavor  vision-system  (owner) 
0 
: initable-instance-variables) 


(defmethod  (vision-system  :initti) 
0 

) 


(defmethod    (vision-system    : scanning) 
0 

) 


(defmethod  (vision-system  :permitted-cell) 
(t-cell) 
(send  graph-terrain  :permitted-cell  t-cell) ) 


(defmethod  (vision-system  :terrain-point) 
(t-cell) 
(send  graph-terrain  : terrain-point  t-cell) ) 
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